﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

using System.Net;
using System.Net.Sockets;
using System.Threading;
using System.Text.RegularExpressions;
using System.Diagnostics;
using System.IO;
using System.Configuration;
using System.Runtime.InteropServices;


namespace CompanyNameCsLineScanner
{
    public delegate void SingleMeasureCallback(double x, double y, double z);
    public delegate void ScanMeasureCallback(double x, double y, double z);
    public delegate void FindSphereCallback(double a, double b, List<OriginData> data);
    public delegate void SendXYZValueCallback(double x, double y, double z);
    public delegate void SendDataCallback(double a, double b, double dis, double light,
                                   double a1, double b1, double dis1, double light1);
    public delegate void SendStateCallback(string deviceState);
    public delegate void SendTemperatureCallback(double temperature1, double temperature2);

    public delegate void SendLevelDataCallback(double level_x, double level_y);

    public delegate void SendSensorCallback(double temperature, double humidity,double pressure);


    public partial class DeviceRadar
    {
        private string main_ip = string.Empty;
        private string singal_ip = string.Empty;
        private string camera_ip = string.Empty;
        private int main_port = 0;
        private int singal_port = 0;
        private string movecard_ip = string.Empty;
        private int movecard_port = 0;
        static public bool showFlagA_Lihgt = false;
        static public bool showFlagB_Lihgt = false;

        private Socket m_socketMain = null;
        static private Socket m_socketSingal = null;
        private byte[] buffer = new byte[1024 * 1024 * 2];
        byte[] recbuffer = new byte[1024 * 1024 * 2];
        byte[] recSingalBuffer = new byte[1024 * 1024 * 2];
        private Socket m_SocketMove = null;
        byte[] recMovecardBuffer = new byte[1024 * 1024 * 2];



        static public bool isScaning = false;

        private double a, a1, b, b1, dis, dis1, light, light1;
        private Int64 focusDis;
        //private int filterCount = 0;
        //private double aV;


        private double m_temperature1, m_temperature2;
        public event SendXYZValueCallback SendXYZ;
        public event SendStateCallback SendStateValue;
        public event SendDataCallback SendData;
        public event SendTemperatureCallback SendTemperature;
        public event SingleMeasureCallback SendSingleMeas;
        public event ScanMeasureCallback SendScanMeas;
        public event FindSphereCallback SendFindSphereData;
        public event SendLevelDataCallback SendLevel;
        public event SendSensorCallback SendSensorData;


        public OriginData m_orignData;
        static public pathPoint scanP1 = new pathPoint();
        static public pathPoint scanP2 = new pathPoint();
        public float pathStep = 1;

        public float moveSpeed;
        public double xstep = 1, ystep = 1;
        public static bool isMoveABComplete = false;
        public bool isMoveAComplete = false;
        public bool isMoveBComplete = false;
        public static bool isAmoving = false;
        public static bool isBmoving = false;
        public double lightValue = 0;
        public bool composateOpen = false;
        public int composateState = 0;
        public bool compensate3D = false;
        public bool polyworks_isON = false;

        public StreamWriter deviceLog;
        public StreamWriter fwcenterData;
        public StreamWriter fycenterData;
        public bool getOrign = false;


        public double g_disK1 = 1;
        public double g_disB1 = 0;
        public double g_disDif1 = 0;
        public double g_disK2 = 1;
        public double g_disB2 = 0;
        public double g_disDif2 = 0;
        public double fyZoro = 0;
        public double cameraOffset = 0;
        public double g_level_x = 0;
        public double g_level_y = 0;

        private double upLimit = 50000;
        private double downLimit = 200;


        public string saveDirectory;

        static OriginData onePoint;
        public OriginData[] tempData;
       // static public OriginData[] mdata = new OriginData[1000000];
        //static public int mdataNum = 0;
        //static public List<OriginData> mScandata = new List<OriginData>();
        static public List<FocusData> m_fcsList = new List<FocusData>();
        static public bool getFocusData = false;
        public int deviceIndex;
        public List<pathPoint> m_Path = new List<pathPoint>();
        int scancount = 0;
        private  Timer socket_timer;

        Queue<double> AData;
        Queue<double> BData;
        Queue<double> DisData;

        Queue<double> AData2;
        Queue<double> BData2;
        Queue<double> DisData2;


        //public Int64 AxisAPulseCount = 18000 * 100 * 40;
        //public Int64 AxisBPulseCount = 18000 * 100 * 40;
        public Int64 AxisAPulseCount = 16384000;
        public Int64 AxisBPulseCount = 16384000;
        /// <summary>
        /// 初始化激光雷达对象，加载补偿文件，加载配置文件，加载矩阵文件
        /// </summary>
        /// <param name="ip"></param>
        /// <param name="port"></param>
        /// <param name="signal_ip"></param>
        /// <param name="signal_port"></param>
        /// <param name="index"></param>
        public DeviceRadar(string ip, int port, string signal_ip, int signal_port, int index)
        {
            //this.main_ip = ip;
            //this.main_port = port;
            //this.singal_ip = signal_ip;
            //this.singal_port = signal_port;
            this.deviceIndex = index;
            ReadCompensateList();//读取补偿文件
            LoadConfig();
            Class_Filter.loadMatrix();
            //for (int i = 0; i < 99; i++)
            //{
            //    AData.Enqueue(0);
            //    BData.Enqueue(0);
            //}
            //Class_Filter.initFilter();
            //AData = new Queue<double>(Class_Filter.medianCount);
            //BData = new Queue<double>(Class_Filter.medianCount);
            //DisData = new Queue<double>(Class_Filter.medianCount);
            //for (int i = 0; i < Class_Filter.medianCount1; i++)
            //{
            //    AData.Enqueue(0);
            //    BData.Enqueue(0);
            //    DisData.Enqueue(0);
            //}
            AData = new Queue<double>(2*Class_Filter.medianCount1+1);
            BData = new Queue<double>(2*Class_Filter.medianCount1+1);
            DisData = new Queue<double>(2 * Class_Filter.medianCount1 + 1);
            for (int i = 0; i < 2 * Class_Filter.medianCount1 + 1; i++)
            {
                AData.Enqueue(0);
                BData.Enqueue(0);
                DisData.Enqueue(0);
            }
            AData2 = new Queue<double>(2 * Class_Filter.medianCount2 + 1);
            BData2 = new Queue<double>(2 * Class_Filter.medianCount2 + 1);
            DisData2 = new Queue<double>(2 * Class_Filter.medianCount2 + 1);
            for (int i = 0; i < 2 * Class_Filter.medianCount2 + 1; i++)
            {
                AData2.Enqueue(0);
                BData2.Enqueue(0);
                DisData2.Enqueue(0);
            }

        }
        ~DeviceRadar()
        {
            // deviceLog.Close();
        }
        /// <summary>
        /// 加载配置文件，配置文件为LaserRadarDLL.dll.config，
        /// 该文件中包含系数K1、B1、D1、K2、B2、D2
        /// 俯仰轴零位角度、相机偏置角度、水平仪偏置角度、
        /// 俯仰轴、方位轴是否补偿
        /// 三维数据是否补偿
        /// </summary>
        public void LoadConfig()
        {
            Configuration myDllConfig =
                         ConfigurationManager.OpenExeConfiguration(this.GetType().Assembly.Location);

            // Get the appSettings section
            AppSettingsSection myDllConfigAppSettings =
                   (AppSettingsSection)myDllConfig.GetSection("appSettings");
            g_disK1 = Convert.ToDouble(myDllConfigAppSettings.Settings["系数K1"].Value);
            g_disB1 = Convert.ToDouble(myDllConfigAppSettings.Settings["系数B1"].Value);
            g_disDif1 = Convert.ToDouble(myDllConfigAppSettings.Settings["系数D1"].Value);
            g_disK2 = Convert.ToDouble(myDllConfigAppSettings.Settings["系数K2"].Value);
            g_disB2 = Convert.ToDouble(myDllConfigAppSettings.Settings["系数B2"].Value);
            g_disDif2 = Convert.ToDouble(myDllConfigAppSettings.Settings["系数D2"].Value);
            fyZoro = Convert.ToDouble(myDllConfigAppSettings.Settings["俯仰零位"].Value);
            composateState = Convert.ToInt16(myDllConfigAppSettings.Settings["是否补偿"].Value);
            compensate3D = Convert.ToBoolean(myDllConfigAppSettings.Settings["三维补偿"].Value);
            cameraOffset = Convert.ToDouble(myDllConfigAppSettings.Settings["相机偏置角度"].Value);
            g_level_x = Convert.ToDouble(myDllConfigAppSettings.Settings["水平仪x偏置"].Value);
            g_level_y = Convert.ToDouble(myDllConfigAppSettings.Settings["水平仪y偏置"].Value);
            this.main_ip = myDllConfigAppSettings.Settings["控制卡IP"].Value;
            this.main_port = Convert.ToInt16(myDllConfigAppSettings.Settings["控制卡端口"].Value); ;
            this.singal_ip = myDllConfigAppSettings.Settings["信号卡IP"].Value;
            this.singal_port = Convert.ToInt16(myDllConfigAppSettings.Settings["信号卡端口"].Value); 
            this.movecard_ip = myDllConfigAppSettings.Settings["运动卡IP"].Value;
            this.movecard_port = Convert.ToInt16(myDllConfigAppSettings.Settings["运动卡端口"].Value); 

        }
        public DeviceRadar()
        {
            ReadCompensateList();//读取补偿文件
        }
        /// <summary>
        /// 连接控制卡
        /// </summary>
        /// <returns>
        /// 连接成功返回True；连接失败返回Flase。
        /// </returns>
        public bool ConnectToMain()
        {
            try
            {
                //1.0 实例化套接字(IP4寻址地址,流式传输,TCP协议)
                m_socketMain = new Socket(AddressFamily.InterNetwork, SocketType.Stream, ProtocolType.Tcp);
                //2.0 创建IP对象
                IPAddress address = IPAddress.Parse(main_ip);
                //3.0 创建网络端口包括ip和端口
                IPEndPoint endPoint = new IPEndPoint(address, main_port);
                //4.0 建立连接
                //m_socketMain.Connect(endPoint); //用该方法，如果网络不通，会等待20-30s的时间


                //采用异步的方式连接socket，避免网络不通时等待时间过长的问题。
                IAsyncResult result = m_socketMain.BeginConnect(endPoint, null, null);
                result.AsyncWaitHandle.WaitOne(500);

                //5.0 接收数据
                if (result.IsCompleted)
                {
                    m_socketMain.BeginReceive(recbuffer, 0, recbuffer.Length, SocketFlags.None, new AsyncCallback(ReceiveCallback), m_socketMain);
                    LaserRadarLog.WriteLogInfo(typeof(DeviceRadar), "连接网口卡:" + main_ip);
                }
                else
                {
                    LaserRadarLog.WriteLogInfo(typeof(DeviceRadar), "连接网口卡失败");
                    return false;
                }
                return true;
            }
            catch (Exception ex)
            {
                LaserRadarLog.WriteLogError(typeof(DeviceRadar), ex);
                m_socketMain.Close();
                //m_socketMain.Dispose();
                return false;
            }

        }
        /// <summary>
        /// 连接信号卡
        /// </summary>
        /// <returns>
        /// 连接成功返回True；连接失败返回Flase。
        /// </returns>
        public bool ConnectToSingal()
        {
            try
            {                   
                //1.0 实例化套接字(IP4寻址地址,流式传输,TCP协议)
                m_socketSingal = new Socket(AddressFamily.InterNetwork, SocketType.Stream, ProtocolType.Tcp);
                //2.0 创建IP对象
                IPAddress address = IPAddress.Parse(singal_ip);
                //3.0 创建网络端口包括ip和端口
                IPEndPoint endPoint = new IPEndPoint(address, singal_port);
                // m_socketSingal.Disconnect();
                //if(!m_socketSingal.Connected)
                    ////4.0 建立连接
                    //    m_socketSingal.Connect(endPoint);
                    //n++;
                    ////5.0 接收数据
                    //m_socketSingal.BeginReceive(recSingalBuffer, 0, recSingalBuffer.Length, SocketFlags.None, new AsyncCallback(ReceiveSingalCallback), m_socketSingal);
                    //LaserRadarLog.WriteLogInfo(typeof(DeviceRadar), "连接信号卡:" + singal_ip);
                    //m_socketSingal.Send(Encoding.UTF8.GetBytes("FMode0"));
                    //采用异步的方式连接socket，避免网络不通时等待时间过长的问题。
                IAsyncResult result = m_socketSingal.BeginConnect(endPoint, null, null);
                result.AsyncWaitHandle.WaitOne(500);

                //5.0 接收数据
                if (result.IsCompleted)
                {
                    m_socketSingal.BeginReceive(recSingalBuffer, 0, recSingalBuffer.Length, SocketFlags.None, new AsyncCallback(ReceiveSingalCallback), m_socketSingal);
                    LaserRadarLog.WriteLogInfo(typeof(DeviceRadar), "连接信号卡:" + singal_ip);
                }
                else
                {
                    LaserRadarLog.WriteLogInfo(typeof(DeviceRadar), "连接信号卡");
                    return false;
                }
                    
                
                //this.socket_timer..Tick += new System.EventHandler(this.timer1_Tick);
                this.socket_timer = new System.Threading.Timer(Socket_Timer_Tick, null, 1000, 100);

                return true;
            }
            catch (Exception ex)
            {
                LaserRadarLog.WriteLogError(typeof(DeviceRadar), ex);
                //m_socketSingal.Close();
                //m_socketSingal.Dispose();
                return false;
            }

        }

        /// <summary>
        /// 连接运动卡
        /// </summary>
        /// <returns>
        /// 连接成功返回True；连接失败返回Flase。
        /// </returns>
        public bool ConnectToMovecard()
        {
            try
            {
                //1.0 实例化套接字(IP4寻址地址,流式传输,TCP协议)
                m_SocketMove = new Socket(AddressFamily.InterNetwork, SocketType.Stream, ProtocolType.Tcp);
                //2.0 创建IP对象
                IPAddress address = IPAddress.Parse(movecard_ip);
                //3.0 创建网络端口包括ip和端口
                IPEndPoint endPoint = new IPEndPoint(address, movecard_port);
                //4.0 建立连接
                //m_SocketMove.Connect(endPoint);
                ////5.0 接收数据
                //m_SocketMove.BeginReceive(recMovecardBuffer, 0, recMovecardBuffer.Length, SocketFlags.None, new AsyncCallback(ReceiveMovecardCallback), m_SocketMove);
                //LaserRadarLog.WriteLogInfo(typeof(DeviceRadar), "连接运动卡:" + main_ip);
                //return true;

                IAsyncResult result = m_SocketMove.BeginConnect(endPoint, null, null);
                result.AsyncWaitHandle.WaitOne(500);

                //5.0 接收数据
                if (result.IsCompleted)
                {
                    m_SocketMove.BeginReceive(recMovecardBuffer, 0, recMovecardBuffer.Length, SocketFlags.None, new AsyncCallback(ReceiveMovecardCallback), m_SocketMove);
                    LaserRadarLog.WriteLogInfo(typeof(DeviceRadar), "连接运动卡:" + singal_ip);
                }
                else
                {
                    LaserRadarLog.WriteLogInfo(typeof(DeviceRadar), "连接运动卡");
                    return false;
                }
                return true;
            }
            catch (Exception ex)
            {
                LaserRadarLog.WriteLogError(typeof(DeviceRadar), ex);
                m_SocketMove.Close();
                //m_socketMain.Dispose();
                return false;
            }

        }

        public void DisConnect()
        {
            if (m_socketSingal.Connected)
                m_socketSingal.Disconnect(true);

            if (m_socketMain.Connected)
                m_socketMain.Disconnect(true);
        }
        
        /// <summary>
        /// 控制卡接收信息回调函数
        /// </summary>
        /// <param name="result"></param>
        private void ReceiveCallback(IAsyncResult result)
        {
            try
            {
                Socket ts = (Socket)result.AsyncState;
                if (ts.Connected)
                {
                    //int nn = ts.EndReceive(result);
                    //Debug.WriteLine("接收数据量：" + nn);

                    result.AsyncWaitHandle.Close();
                    String str = System.Text.Encoding.Default.
                    GetString(recbuffer);
                    int index = str.IndexOf("FF");
                    if (index == -1)
                    {
                        return;
                    }
                    string receivedData = Encoding.UTF8.GetString(recbuffer, 0, index + 2);
                    //Debug.WriteLine(receivedData);
                    DataProcess(receivedData);
                    
                    //清空数据，重新开始异步接收
                    recbuffer = new byte[recbuffer.Length];
                    ts.BeginReceive(recbuffer, 0, recbuffer.Length, SocketFlags.None, new AsyncCallback(ReceiveCallback), ts);
                }
            }
            catch (Exception ex)
            {
                LaserRadarLog.WriteLogError(typeof(DeviceRadar), ex);
                //遇到连接错误时重新连接，这里是否应该给予提示。
                //ConnectToMain();
            }
        }

        /// <summary>
        /// 运动卡接收信息回调函数
        /// </summary>
        /// <param name="result"></param>
        private void ReceiveMovecardCallback(IAsyncResult result)
        {
            try
            {
                Socket ts = (Socket)result.AsyncState;
                if (ts.Connected)
                {
                    //int nn = ts.EndReceive(result);
                    //Debug.WriteLine("接收数据量：" + nn);

                    result.AsyncWaitHandle.Close();
                    String str = System.Text.Encoding.Default.
                    GetString(recMovecardBuffer);
                    int index = str.IndexOf("FF");
                    if (index == -1)
                    {
                        return;
                    }
                    string receivedData = Encoding.UTF8.GetString(recMovecardBuffer, 0, index + 2);
                    //Debug.WriteLine(receivedData);
                    DataProcess(receivedData);

                    //清空数据，重新开始异步接收
                    recbuffer = new byte[recMovecardBuffer.Length];
                    ts.BeginReceive(recMovecardBuffer, 0, recMovecardBuffer.Length, SocketFlags.None, new AsyncCallback(ReceiveMovecardCallback), ts);
                }
            }
            catch (Exception ex)
            {
                LaserRadarLog.WriteLogError(typeof(DeviceRadar), ex);
                //遇到连接错误时重新连接，这里是否应该给予提示。
                //ConnectToMain();
            }
        }


        //下面这个函数貌似没什么用，receiveData()
        //void receiveData(IAsyncResult result)
        //{
        //    Socket ts = (Socket)result.AsyncState;
        //    ts.EndReceive(result);
        //    result.AsyncWaitHandle.Close();
        //    string receData = Encoding.UTF8.GetString(recSingalBuffer);
        //    //清空数据，重新开始异步接收
        //    recSingalBuffer = new byte[recSingalBuffer.Length];
        //    ts.BeginReceive(recSingalBuffer, 0, recSingalBuffer.Length, SocketFlags.None, new AsyncCallback(receiveData), ts);
        //}
        /// <summary>
        /// 信号卡接收信息回调函数
        /// </summary>
        /// <param name="result"></param>
        void ReceiveSingalCallback(IAsyncResult result)
        {
            try
            {
                socket_current++;
                if (socket_current == 10000)
                {
                    socket_current = 0;
                    Debug.WriteLine("socket达到10000");
                }

                Socket ts = (Socket)result.AsyncState;
                if (ts.Connected)
                {
                    int nn = ts.EndReceive(result);
                   // Debug.WriteLine("接收数据量：" + nn);
                    if (nn == 16)
                    {
                        byte[] temp = new byte[16];
                        Array.Copy(recSingalBuffer, 0, temp, 0, 16);
                        string data = System.Text.Encoding.ASCII.GetString(temp);
                        SendStateValue(data);
                        //return;
                    }
                    else if (nn == 13)
                    {
                        byte[] temp = new byte[13];
                        Array.Copy(recSingalBuffer, 0, temp, 0, 13);
                        string data = System.Text.Encoding.ASCII.GetString(temp);
                        SendStateValue(data);
                    }
                    else
                    {
                        //Debug.WriteLine(nn);
                        switch (SignalIndex)
                        {
                            case 0:
                                //int num = ts.EndReceive(result) / 78;
                                int num = nn / 78;
                                for (int i = 0; i < num; i++)
                                {
                                    byte[] temp = new byte[78];
                                    Array.Copy(recSingalBuffer, i * 78, temp, 0, 78);
                                    string data = System.Text.Encoding.ASCII.GetString(temp);
                                    if (data.Substring(0, 8) == "aaab240c" && data.Substring(70, 8) == "ff000000")
                                    {
                                        SingalDataProcess(data);
                                    }
                                }
                                break;
                            case 1:
                                int count = nn / 34;
                                for (int i = 0; i < count; i++)
                                {
                                    byte[] temp = new byte[34];
                                    Array.Copy(recSingalBuffer, i * 34, temp, 0, 34);
                                    string data = System.Text.Encoding.ASCII.GetString(temp);
                                    if (data.Substring(0, 8) == "abc123de" && data.Substring(26, 8) == "levelxyt")
                                    {
                                        LevelDataProcess(data);
                                    }
                                }
                                break;

                            default:
                                break;
                        }
                    }
                }
                result.AsyncWaitHandle.Close();
                //清空数据，重新开始异步接收
                recSingalBuffer = new byte[recSingalBuffer.Length];
                ts.BeginReceive(recSingalBuffer, 0, recSingalBuffer.Length, SocketFlags.None, new AsyncCallback(ReceiveSingalCallback), ts);
                //socket_last = socket_current;
                
            }
            catch (Exception ex)
            {
                LaserRadarLog.WriteLogError(typeof(DeviceRadar), ex);
                //ConnectToSingal();
            }
            
        }

        int timer_count = 0;
        int socket_current = 0;
        int socket_last = 0;
        /// <summary>
        /// Socket定时器，监控网络状态。若连接断开，给予提示信息；
        /// </summary>
        /// <param name="theObj"></param>
        private void Socket_Timer_Tick(object theObj)
        {         
            if (socket_last == socket_current)
            {
                timer_count++;
                if (timer_count == 10)
                {
                    try
                    {

                        //socket_timer.Dispose();
                        Debug.WriteLine("连接已断开");
                        //SendStateValue("连接已断开");

                       // m_socketSingal.Disconnect(true);
                       // m_socketMain.Disconnect(true);
                    }
                    catch (Exception ex)
                    {
                        Debug.WriteLine(ex);
                    }

                }
            }
            else
            {
                socket_last = socket_current;
                timer_count = 0;
            }

        }
        ////将88位字符转换成字符串
        //static public string StringConvert(string[] sourceString)
        //{
        //    string afterConvert = null;
        //    for (int i = 0; i < 78; i++)
        //    {
        //        afterConvert += sourceString[i];
        //    }
        //    return afterConvert;
        //}

        /// <summary>
        /// 解析信号板的数据
        /// </summary>
        /// <param name="data">
        /// data是78位的数据，包含上升沿、下降沿的距离和光强，方位角度、俯仰角度、调焦距离
        /// </param>
        private void SingalDataProcess(string data)
        {
            try
            {
                //*****对80位数据进行解析
                string computeTemp = data.Substring(0, 8);//aaab240c                   
                computeTemp = data.Substring(8, 8);
                dis = computeDis(computeTemp);
                computeTemp = data.Substring(16, 8);
                dis1 = computeDis2(computeTemp);
                computeTemp = data.Substring(24, 8);
                light = computeFocusDis(computeTemp);
                computeTemp = data.Substring(32, 8);
                light1 = computeFocusDis(computeTemp);
                computeTemp = data.Substring(40, 10);
                a = computeFW(computeTemp);
                computeTemp = data.Substring(50, 10);
                b = computeFY(computeTemp);

                computeTemp = data.Substring(60, 10);
                focusDis = Convert.ToInt64(computeTemp);

                //方位和俯仰进行补偿，并添加俯仰零位偏置
                b = disCompensateB(b) - fyZoro;
                a = disCompensateA(a);
                b1 = disCompensateB(b1) - fyZoro;
                a1 = disCompensateA(a1);
                onePoint = new OriginData(a, b, dis, light);
                if (getScanData == true)
                {
                    findCenterData.Add(new OriginData(a, b, (dis + dis1) / 2, light));
                }
                double distance = (dis + dis1) / 2;
                if(SendData != null)
                    this.SendData(a, b, distance, light, focusDis, m_temperature1, dis1, light1);


                if (FilterFlag)
                {
                    //AData.Dequeue();
                    //BData.Dequeue();
                    //AData.Enqueue(a);
                    //BData.Enqueue(b);
                    //DisData.Dequeue();
                    //DisData.Enqueue(Class_Filter.singleMedianFilter(distance));
                    ////DisData.Enqueue(distance);
                    //a = AData.ToArray()[Class_Filter.medianCount1 + Class_Filter.medianCount2 + 1];
                    //b = BData.ToArray()[Class_Filter.medianCount1 + Class_Filter.medianCount2 + 1];
                    //distance = DisData.ToArray()[Class_Filter.medianCount1 + Class_Filter.medianCount2 + 1];
                    AData.Dequeue();
                    BData.Dequeue();
                    AData.Enqueue(a);
                    BData.Enqueue(b);
                    DisData.Dequeue();
                    DisData.Enqueue(distance);
                    double[] array1 = DisData.ToArray();
                    System.Array.Sort(array1);
                    distance= array1[Class_Filter.medianCount1  + 1];
                    //DisData.Enqueue(distance);
                    a = AData.ToArray()[Class_Filter.medianCount1 + 1];
                    b = BData.ToArray()[Class_Filter.medianCount1 + 1];
                    //distance = DisData.ToArray()[Class_Filter.medianCount1 + 1];

                    AData2.Dequeue();
                    BData2.Dequeue();
                    AData2.Enqueue(a);
                    BData2.Enqueue(b);
                    DisData2.Dequeue();
                    DisData2.Enqueue(distance);
                    double[] array2 = DisData2.ToArray();
                    System.Array.Sort(array2);
                    distance = array2[Class_Filter.medianCount2 + 1];
                    //DisData.Enqueue(distance);
                    a = AData2.ToArray()[Class_Filter.medianCount2 + 1];
                    b = BData2.ToArray()[Class_Filter.medianCount2 + 1];
                }
                //滤波算法
                //AData.Dequeue();
                //BData.Dequeue();
                //AData.Enqueue(a);
                //BData.Enqueue(b);
                //double distance =Class_Filter._99filter( (dis + dis1) / 2);

                //a = AData.ToArray()[49];
                //b = BData.ToArray()[49];
                ////double distance = (Class_Filter._99filter(dis) + Class_Filter._99filter(dis1)) / 2;

                //this.setData(a, b, distance, light, focusDis, m_temperature1, dis1, light1);

                //单点扫描时，用如下代码实现记录1000点
                if (isgettingOne)
                {
                    pointList.Add(new OriginData(a, b, dis, light));
                    if (pointList.Count == 1000)
                    {
                        isgettingOne = false;
                        isgetComplete = true;
                    }
                }
                if (getFocusData)
                {
                    m_fcsList.Add(new FocusData(light, focusDis));                       
                }
                //区域扫描
                if (isScan)
                {
                    double x = 0, y = 0, z = 0;
                    //过滤掉噪点
                    if (distance > downLimit && distance < upLimit)
                    {
                        if(compensate3D)
                            ComputeXYZ(a, b, distance, ref x, ref y, ref z);
                        else
                            ComputeXYZ_noComse(a, b, distance, ref x, ref y, ref z); 
                        //mdataNum++;
                        if (SendXYZ != null)
                            SendXYZ(x, y, z);
                        if (SendScanMeas != null)
                            SendScanMeas(x, y, z);                           
                    }
                }
                if (getOrign)
                {
                    deviceLog.WriteLine(a + "," + b + "," + distance + "," + light + "," + dis1 + "," + light1 + "," + m_temperature1 + "," + DateTime.Now.ToLongTimeString());
                }
                
            }
            catch (Exception ex)
            {
                LaserRadarLog.WriteLogError(typeof(DeviceRadar), ex);
                Debug.WriteLine("信号处理出现错误：" + ex.ToString());
            }
        }

        public int numOfReceivedPoint = 0;
        List<OriginData> pointList = new List<OriginData>();
        public bool isgettingOne = false;
        public bool isgetComplete = false;
        public int num = 1000;

        public bool FilterFlag = true;

        /// <summary>
        /// 读取1000个原始数据，
        /// </summary>
        /// <returns>
        /// 返回值为对1000个数据进行3sigma滤波后的一个数据对象
        /// </returns>
        private OriginData getOneThousandData()
        {
            int n = pointList.Count;//与getOneData()函数的区别在这里

            double[] origin_a_array = new double[n];
            double[] origin_b_array = new double[n];
            double[] origin_dis_array = new double[n];
            double[] origin_light_array = new double[n];

            for (int i = 0; i < n; i++)
            {
                OriginData od = pointList[i];
                origin_a_array[i] = od.a;
                origin_b_array[i] = od.b;
                origin_dis_array[i] = od.dis;
                origin_light_array[i] = od.light;
            }
            double new_a = Class_Filter.sigma3Filter(origin_a_array);
            double new_b = Class_Filter.sigma3Filter(origin_b_array);
            double new_dis = Class_Filter.sigma3Filter(origin_dis_array);
            double new_light = Class_Filter.sigma3Filter(origin_light_array);
            OriginData oneData = new OriginData(new_a, new_b, new_dis, new_light);
            return oneData;
        }
      
        /// <summary>
        /// 读取一个原始点数据
        /// </summary>
        /// <returns>
        /// 返回值为对1000给点数据进行滤波后的一个数据对象
        /// </returns>
        //getOneData中应用3sigma滤波
        public OriginData GetOneData()
        {
            int num = 1000;
            double[] origin_a_array = new double[num];
            double[] origin_b_array = new double[num];
            double[] origin_dis_array = new double[num];
            double[] origin_light_array = new double[num];
            for (int i = 0; i < num; i++)
            {
                OriginData od = onePoint;
                origin_a_array[i] = od.a;
                origin_b_array[i] = od.b;
                origin_dis_array[i] = od.dis;
                origin_light_array[i] = od.light;
            }

            double new_a = Class_Filter.sigma3Filter(origin_a_array);
            double new_b = Class_Filter.sigma3Filter(origin_b_array);
            double new_dis = Class_Filter.sigma3Filter(origin_dis_array);
            double new_light = Class_Filter.sigma3Filter(origin_light_array);
            OriginData oneData = new OriginData(new_a, new_b, new_dis, new_light);
            return oneData;
        }
        /// <summary>
        /// 获取一个笛卡尔坐标点
        /// </summary>
        /// <returns>
        /// 返回一个SAPoint点
        /// </returns>
        public SAPoint GetOnePoint()
        {
            OriginData oneData = getOneThousandData();
            double x = 0, y = 0, z = 0;
            if (compensate3D)
                ComputeXYZ(oneData.a, oneData.b, oneData.dis, ref x, ref y, ref z);
            else
                ComputeXYZ_noComse(oneData.a, oneData.b, oneData.dis, ref x, ref y, ref z);
            SAPoint point = new SAPoint(x, y, z);
            return point;
        }
        private double computeFocusDis(string focusDis)
        {
            if (focusDis != null)
            {
                Int64 foucsInt = Convert.ToInt64(focusDis);
                return foucsInt;
            }
            else
                return 0;
        }
        private double computeFY(string fy)
        {
            try
            {
                if (fy != null)
                {
                    Int64 fyInt = Convert.ToInt64(fy);
                    if (fyInt > (AxisBPulseCount))
                    {
                        Int64 value = fyInt ^ 0xFFFFFFFF;
                        fyInt = -value;
                    }
                    double realFyD = (-fyInt * 360.0) / Convert.ToDouble(AxisBPulseCount);
                    double fyD = realFyD % 360;
                    return fyD;
                }
                else
                    return 0;
            }
            catch (Exception ex)
            {
                LaserRadarLog.WriteLogError(typeof(DeviceRadar), ex);
                return 0;
            }
        }
        private double computeFW(string fw)
        {

            if (fw != null)
            {
                Int64 fwInt = Convert.ToInt64(fw);
                Int64 count = AxisAPulseCount;
                if (fwInt > count * 2)
                {
                    Int64 value = fwInt ^ 0xFFFFFFFF;
                    fwInt = -value;
                }
                double realFwD = (fwInt * 360.0) / Convert.ToDouble(AxisAPulseCount);
                double fwD = realFwD % 360;
                return fwD;
            }
            else
                return 0;
        }

        private double computeDis(string dis)
        {
            if (dis != null)
            {
                Int64 disInt = Convert.ToInt64(dis);
                double disFD = g_disK1 * disInt + g_disB1 + g_disDif1;
                double resdisFD = disFD * 0.001;
                return resdisFD;
            }
            else
                return 0;
        }
        private double computeDis2(string dis)
        {
            if (dis != null)
            {
                Int64 disInt = Convert.ToInt64(dis);
                double disFD = g_disK2 * disInt + g_disB2 + g_disDif2;
                double resdisFD = disFD * 0.001;
                return resdisFD;
            }
            else
                return 0;
        }
        //解析温度数据
        private void DataProcess(string data)
        {
            try
            {
                string temp = data.Substring(0, 8);
                if (temp == "AA AB AA")//温度数据格式 AA AB AA 18 59 BB 18 54 FF ;  AA、BB后4位为数据
                {
                    string tempe1 = data.Substring(9, 2) + data.Substring(12, 2);
                    string tempe2 = data.Substring(18, 2) + data.Substring(21, 2);
                    double tempe1d = Convert.ToDouble(tempe1) / 4096 * 3.3;
                    double tempe2d = Convert.ToDouble(tempe2) / 4096 * 3.3;
                    tempe1d = (tempe1d - 0.1) / 2.9 * 20 + 15;
                    tempe2d = (tempe2d - 0.1) / 2.9 * 20 + 15;
                    m_temperature1 = tempe1d;
                    m_temperature2 = tempe2d;
                    if (SendTemperature != null)
                        SendTemperature(tempe1d, tempe2d);
                }
                if (temp == "AA AB ZS")//温度、湿度、气压
                {
                    string tempe1 = data.Substring(9, 6);
                    string tempe2 = data.Substring(15, 6);
                    string tempe3 = data.Substring(21, 6);
                    double temperature = Convert.ToDouble(tempe1) / 10;
                    double humidity = Convert.ToDouble(tempe2) / 10;
                    double pressure = Convert.ToDouble(tempe3) / 10;
                  
                    if (SendSensorData != null)
                        SendSensorData(temperature, humidity, pressure);
                }
                if (temp == "AA AB OS")//光开关 AA AB OS 0X FF，X：取值0、1、2、3、4，代表开启哪个通道
                {
                    string tempChannel = data.Substring(9, 2);
                    int channelIndex = Convert.ToInt32(tempChannel);
                    string showMessage = "光通道："+channelIndex+"开启";
                    if (SendStateValue != null)
                        SendStateValue(showMessage);//向窗体传送事件信息
                    LaserRadarLog.WriteLogInfo(typeof(DeviceRadar), showMessage);
                }
                else
                {
                    string showMessage = null;
                    switch (data)
                    {
                        case CK_TRUNTABLE_INIT_RE:
                            //角度清零
                            Thread.Sleep(1500);
                            SetZero();
                            Thread.Sleep(1500);
                            OriginData od = GetOneData();
                            if (od.a > 0.0002)
                                SetZero();
                            showMessage = "初始化完成";
                            break;
                        case CK_TRUNTABLE_SM_WORK_COMPLETE:
                            showMessage = "区域扫描完成";
                            isScaning = false;
                            break;
                        case CK_NETPANEL_TJ_End:
                            showMessage = "调焦完成";
                            break;
                        case CK_AMOVE_COMPLETE:

                            showMessage = "方位指向完成";
                            isMoveAComplete = true;
                            isAmoving = false;
                            SendAMoveComplete();
                            break;
                        case CK_BMOVE_COMPLETE:

                            showMessage = "俯仰指向完成";
                            isMoveBComplete = true;
                            isBmoving = false;
                            SendBMoveComplete();
                            break;
                        //case CK_ABMOVE_COMPLETE:
                        //    showMessage = "球心指向完成";
                        //    isMoveABComplete = true;
                        //    break;
                        case CK_NETPANEL_TJ_UP_POS:
                            showMessage = "调焦上限位";
                            break;
                        case CK_NETPANEL_TJ_DOWN_POS:
                            showMessage = "调焦下限位";
                            break;
                        case CK_NETPANEL_TJ_DownLimit:
                            m_fcsList.Clear();
                            getFocusData = true;
                            showMessage = "调焦下限位";
                            break;
                        case CK_NETPANEL_TJ_UpLimit:
                            getFocusData = false;
                            ComputeFocusMax();
                            showMessage = "调焦上限位";
                            break;
                        case CK_AMOVE_RECEIVED:
                            MessageReceived = true;
                            break;
                        case CK_BMOVE_RECEIVED:
                            MessageReceived = true;
                            break;

                        case CK_TRUNTABLE2_SPIRALSCAN_COMPLETE:
                            showMessage = "螺旋形扫描完成";
                            break;
                        case CK_TRUNTABLE2_HOLESCAN_COMPLETE:
                            showMessage = "孔扫描完成";
                            break;
                        case CK_TRUNTABLE2_RECTANGLESCAN_COMPLETE:
                            showMessage = "区域扫描完成";
                            break;
                        case CK_TRUNTABLE2_POWERON:
                            showMessage = "设备使能";
                            break;
                        case CK_ABMOVE_SOLO_COMPLETE:
                            showMessage = "指令定位完成";
                            break;
                        case CK_ABMOVE_TOGETHER_COMPLETE:
                            showMessage = "脉冲定位完成";
                            break;
                        case CK_MOVE_STOP_FLAG:
                            showMessage = "运动已停止";
                            break;
                        case CK_MOVE_SETSCALE_COMPLETE:
                            showMessage = "比例系数设置成功";
                            break;
                        case CK_TRUNTABLE2_RECTANGLESCAN_REQUESTEDGES:
                            ScanAddPoints();
                            showMessage = "添加扫描边界点";
                            break;                           
                        default:
                            break;
                    }
                    if (SendStateValue != null)
                        SendStateValue(showMessage);//向窗体传送事件信息
                    LaserRadarLog.WriteLogInfo(typeof(DeviceRadar), showMessage);
                }
            }
            catch(Exception ex)
            {
                Debug.WriteLine(ex.ToString());
            }
        }
        /// <summary>
        /// 计算调焦过程中的最大值，并移动到该位置
        /// </summary>
        private void ComputeFocusMax()
        {
            double light = 0;
            Int64 dis = 0;
            foreach (FocusData fd in m_fcsList)
            {
                if (fd.light > light)
                {
                    light = fd.light;
                    dis = fd.focusDis;
                }
            }
            SetFocusDis(dis / 4);
        }
        /// <summary>
        /// 初始化设备（各轴回零）
        /// </summary>
        public void InitDevice()
        {
            string sendMessage = CK_TRUNTABLE_INIT;
            if (m_SocketMove.Connected)
            {
                m_SocketMove.Send(Encoding.UTF8.GetBytes(sendMessage));
                LaserRadarLog.WriteLogInfo(typeof(DeviceRadar), "初始化开始");
            }
        }

        public void PowerOn()
        {
            string sendMessage = CK_TRUNTABLE2_POWERON;
            if (m_SocketMove.Connected)
            {
                m_SocketMove.Send(Encoding.UTF8.GetBytes(sendMessage));
                LaserRadarLog.WriteLogInfo(typeof(DeviceRadar), "设备使能");
            }
        }
        /// <summary>
        /// 方位轴正向步进运动
        /// </summary>
        /// <param name="fast"> 如果fast=true，快速运动模式；如果fast =false,慢速模式</param>
        public void AxisAmovePos(bool fast)
        {
            if (fast)
            {
                string sendMessage = CK_TRUNTABLE_LEFT_FAST_PRESS;
                if (m_SocketMove.Connected)
                    m_SocketMove.Send(Encoding.UTF8.GetBytes(sendMessage));
            }
            else
            {
                string sendMessage = CK_TRUNTABLE_LEFT_SLOW_PRESS;
                if (m_SocketMove.Connected)
                    m_SocketMove.Send(Encoding.UTF8.GetBytes(sendMessage));
            }
        }
        /// <summary>
        /// 方位轴负向步进运动
        /// </summary>
        /// <param name="fast">如果fast=true，快速运动模式；如果fast =false,慢速模式</param>
        public void AxisAmoveNeg(bool fast)
        {
            if (fast)
            {
                string sendMessage = CK_TRUNTABLE_RIGHT_FAST_PRESS;
                if (m_SocketMove.Connected)
                    m_SocketMove.Send(Encoding.UTF8.GetBytes(sendMessage));
            }
            else
            {
                string sendMessage = CK_TRUNTABLE_RIGHT_SLOW_PRESS;
                if (m_SocketMove.Connected)
                    m_SocketMove.Send(Encoding.UTF8.GetBytes(sendMessage));
            }
        }
        /// <summary>
        /// 俯仰轴正向步进运动
        /// </summary>
        /// <param name="fast">如果fast=true，快速运动模式；如果fast =false,慢速模式</param>
        public void AxisBmovePos(bool fast)
        {
            if (fast)
            {
                string sendMessage = CK_TRUNTABLE_UP_FAST_PESS;
                if (m_socketMain.Connected)
                    m_socketMain.Send(Encoding.UTF8.GetBytes(sendMessage));
            }
            else
            {
                string sendMessage = CK_TRUNTABLE_UP_SLOW_PRESS;
                if (m_socketMain.Connected)
                    m_socketMain.Send(Encoding.UTF8.GetBytes(sendMessage));
            }
        }
        /// <summary>
        /// 俯仰轴负向步进运动
        /// </summary>
        /// <param name="fast">如果fast=true，快速运动模式；如果fast =false,慢速模式</param>
        public void AxisBmoveNeg(bool fast)
        {
            if (fast)
            {
                string sendMessage = CK_TRUNTABLE_DOWN_FAST_PRESS;
                if (m_socketMain.Connected)
                    m_socketMain.Send(Encoding.UTF8.GetBytes(sendMessage));
            }
            else
            {
                string sendMessage = CK_TRUNTABLE_DOWN_SLOW_PRESS;
                if (m_socketMain.Connected)
                    m_socketMain.Send(Encoding.UTF8.GetBytes(sendMessage));
            }
        }
        /// <summary>
        /// 俯仰轴停止步进运动
        /// </summary>
        public void AxisBstop()
        {
            string sendMessage = CK_TRUNTABLEUPDOWN_STOP;
            if (m_socketMain.Connected)
                m_socketMain.Send(Encoding.UTF8.GetBytes(sendMessage));
        }
        /// <summary>
        /// 方位轴停止步进运动
        /// </summary>
        public void AxisAstop()
        {
            string sendMessage = CK_TRUNTABLELERI_STOP;
            if (m_socketMain.Connected)
                m_socketMain.Send(Encoding.UTF8.GetBytes(sendMessage));
        }
        /// <summary>
        /// 打开指示激光器
        /// </summary>
        public void LightOn()
        {
            //string sendMessage = CK_LIGHT_ON;
            string sendMessage = CK_APPENDIX_REDLASER_ON;
            if (m_socketMain.Connected)
            {
                m_socketMain.Send(Encoding.UTF8.GetBytes(sendMessage));
                LaserRadarLog.WriteLogInfo(typeof(DeviceRadar), "指示激光开");
            }
        }
        /// <summary>
        /// 关闭指示激光器
        /// </summary>
        public void LightOff()
        {
            //string sendMessage = CK_LIGHT_OFF;
            string sendMessage = CK_APPENDIX_REDLASER_OFF;
            if (m_socketMain.Connected)
            {
                m_socketMain.Send(Encoding.UTF8.GetBytes(sendMessage));
                LaserRadarLog.WriteLogInfo(typeof(DeviceRadar), "指示激光关");
            }
        }
        /// <summary>
        /// 打开衰减器
        /// </summary>
        public void Attenuator_ON()
        {
            string sendMessage = CK_APPENDIX_ATTENUATOR_ON;
            if (m_socketMain.Connected)
            {
                m_socketMain.Send(Encoding.UTF8.GetBytes(sendMessage));
                LaserRadarLog.WriteLogInfo(typeof(DeviceRadar), "衰减器开");
            }
        }
        /// <summary>
        /// 关闭衰减器
        /// </summary>
        public void Attenuator_OFF()
        {
            string sendMessage = CK_APPENDIX_ATTENUATOR_OFF;
            if (m_socketMain.Connected)
            {
                m_socketMain.Send(Encoding.UTF8.GetBytes(sendMessage));
                LaserRadarLog.WriteLogInfo(typeof(DeviceRadar), "衰减器关");
            }
        }
        /// <summary>
        /// 扫描开始（弃用）
        /// </summary>
        /// <param name="step"></param>
        /// <param name="speed"></param>
        //该指令不用了
        public void ScanStart(float step, float speed)
        {
            int st = (int)(step * 10);
            int sp = (int)(speed * 10);
            string strStep = st.ToString("x2");
            string strSpeed = sp.ToString("x2");
            string sendMessage = "AA AB 51 02 " + strStep + " " + strSpeed + " FF";
            if (m_socketMain.Connected)
            {
                m_socketMain.Send(Encoding.UTF8.GetBytes(sendMessage));
                isScaning = true;
            }
        }
        /// <summary>
        /// 扫描暂停（弃用）
        /// </summary>
        //该指令不用了
        public void ScanPause()
        {
            string sendMessage = CK_TRUNTABLE_SM_WORK_PAUSE;
            if (m_socketMain.Connected)
                m_socketMain.Send(Encoding.UTF8.GetBytes(sendMessage));
        }
        /// <summary>
        /// 扫描停止（弃用）
        /// </summary>
        //该指令不用了
        public void ScanStop()
        {
            string sendMessage = CK_TRUNTABLE_SM_WORK_END;
            if (m_socketMain.Connected)
                m_socketMain.Send(Encoding.UTF8.GetBytes(sendMessage));
        }
        /// <summary>
        /// 设置矩形扫描起始点
        /// </summary>
        public void SetScanStartPos()
        {
            scanP1 = new pathPoint(a, b);
        }
        /// <summary>
        ///  设置矩形扫描终止点
        /// </summary>

        public void SetScanEndPos()
        {
            scanP2 = new pathPoint(a, b);
        }
        /// <summary>
        /// 调焦
        /// </summary>
        public void Focus()
        {
            string sendMessage = CK_NETPANEL_TJ_Start;
            if (m_socketMain.Connected)
            {
                m_socketMain.Send(Encoding.UTF8.GetBytes(sendMessage));
                LaserRadarLog.WriteLogInfo(typeof(DeviceRadar), "开始调焦");
            }
        }
        /// <summary>
        /// 移动到焦距最佳位置
        /// </summary>
        /// <param name="dis"></param>
        private void SetFocusDis(Int64 dis)
        {
            string stringSplit = Regex.Replace(dis.ToString("X8"), @".{2}", "$0 ");//将string每两位分隔并添加空格
            string sendMessage = "AA AB 17 76 " + stringSplit + "FF";
            if (m_socketMain.Connected)
            {
                m_socketMain.Send(Encoding.UTF8.GetBytes(sendMessage));
                LaserRadarLog.WriteLogInfo(typeof(DeviceRadar), "设置调焦位置：" + dis);
            }
        }
        /// <summary>
        /// 设置调焦阈值
        /// </summary>
        /// <param name="focusValue">调焦电机位置 </param>
        public void SetFocusValue(string focusValue)
        {
            m_socketSingal.Send(Encoding.UTF8.GetBytes(focusValue));

        }
        /// <summary>
        /// 向远处调焦
        /// </summary>
        public void FocusUpMove()
        {
            string sendMessage = CK_NETPANEL_TJ_UP;
            m_socketMain.Send(Encoding.UTF8.GetBytes(sendMessage));
        }
        /// <summary>
        /// 向近处调焦
        /// </summary>
        public void FocusDownMove()
        {
            string sendMessage = CK_NETPANEL_TJ_DOWN;
            if (m_socketMain.Connected)
                m_socketMain.Send(Encoding.UTF8.GetBytes(sendMessage));
        }
        /// <summary>
        /// 停止调焦
        /// </summary>
        public void FocusStopMove()
        {
            string sendMessage = CK_NETPANEL_TJ_STOP;
            m_socketMain.Send(Encoding.UTF8.GetBytes(sendMessage));
        }
        /// <summary>
        /// 角度清零
        /// </summary> 
        public void SetZero()
        {
            if(m_socketSingal !=null)
            { 
                m_socketSingal.Send(Encoding.UTF8.GetBytes(CK_NETPANEL_INIT_DONE));
                m_socketSingal.Send(Encoding.UTF8.GetBytes("FMode0"));//开始发送数据
            }
        }
        //该指令不用了
        private bool connectToCamera()
        {
            //m_socketSingal.Send(Encoding.UTF8.GetBytes("AAAB8000FF"));//发送该指令后启动信号板上传数据
            return true;
        }
        public bool MessageReceived = false;
        public string Message;
        /// <summary>
        /// 发送消息
        /// </summary>
        private void sendMessageProcess()
        {
            //m_socketMain.Send(Encoding.UTF8.GetBytes(Message));
            while (!MessageReceived)
            {
                if (m_socketMain.Connected)
                {
                    m_socketMain.Send(Encoding.UTF8.GetBytes(Message));
                    Thread.Sleep(1000);
                }
            }
        }
        /// <summary>
        /// 方位轴运动
        /// </summary>
        /// <param name="angleA">目标位置</param>
        /// <param name="speed">运动速度</param>
        public void AmoveTo(float angleA, double speed)
        {
            int speedA = Convert.ToInt32(speed * 10 * AxisAPulseCount / 360);
            isAmoving = true;
            if (composateState == 1)
                angleA = (float)compensateA(angleA);
            bool isreversFw = false;
            if (angleA < 0)
            {
                isreversFw = true;
                angleA = -angleA;
            }
            Int32 angle = (Int32)(angleA * (AxisAPulseCount) / 360);
            string strHex = angle.ToString("x6");
            if (isreversFw)
                strHex = "80" + strHex;
            else
                strHex = "00" + strHex;
            if (speedA < 0)
                speedA = 1000;
            string strHexSpeedA = speedA.ToString("x8");
            strHex += strHexSpeedA;
            string stringSplit = Regex.Replace(strHex, @".{2}", "$0 ");//将string每两位分隔并添加空格
            string sendMessage = "AA AB 28 08 " + stringSplit + "FF";
            if (m_socketMain.Connected)
            {
                //m_socketMain.Send(Encoding.UTF8.GetBytes(sendMessage));
                Message = sendMessage;
                MessageReceived = false;
                Task sendingTask = new Task(sendMessageProcess);
                sendingTask.Start();
                //m_socketMain.Send(Encoding.UTF8.GetBytes(sendMessage));
                scancount++;
                Debug.WriteLine(scancount + "," + sendMessage);
            }
        }
        /// <summary>
        /// 俯仰轴运动
        /// </summary>
        /// <param name="angleB">目标位置</param>
        /// <param name="speed">运动速度</param>
        public void BmoveTo(float angleB, double speed)
        {
            int speedB = Convert.ToInt32(speed * 10 * AxisBPulseCount / 360);
            isBmoving = true;
            angleB += (float)fyZoro;

            if (composateState == 1)
                angleB = (float)compensateB(angleB);

            bool isreversFy = false;
            if (angleB < 0)
            {
                isreversFy = true;
                angleB = -angleB;
            }
            Int32 angle = (Int32)(angleB * (AxisBPulseCount) / 360);
            string strHex = angle.ToString("x6");
            if (isreversFy)
                strHex = "80" + strHex;
            else
                strHex = "00" + strHex;
            if (speedB < 0)
                speedB = 1000;
            string strHexSpeedB = speedB.ToString("x8");
            strHex += strHexSpeedB;
            string stringSplit = Regex.Replace(strHex, @".{2}", "$0 ");//将string每两位分隔并添加空格
            string sendMessage = "AA AB 29 08 " + stringSplit + "FF";
            if (m_socketMain.Connected)
            {
                //m_socketMain.Send(Encoding.UTF8.GetBytes(sendMessage));
                Message = sendMessage;
                MessageReceived = false;
                Task sendingTask = new Task(sendMessageProcess);
                sendingTask.Start();
                //m_socketMain.Send(Encoding.UTF8.GetBytes(sendMessage));
                scancount++;
                Debug.WriteLine(scancount + "," + sendMessage);
            }
            
        }

#region 运动卡指令
        /// <summary>
        /// 指令定位或称精确定位，两个轴独立运动，分别设置速度
        /// </summary>
        /// <param name="angleA">目标位置</param>
        /// <param name="speed">运动速度</param>
        public bool MoveAB2(double angleA, double angleB, double speedA,double speedB)
        {
            try
            {
                isAmoving = true;
                if (composateState == 1)
                {
                    angleA = (float)compensateA(angleA);
                    angleB = (float)compensateB(angleB);
                }

                Int32 angle = (Int32)(angleA * AxisAPulseCount / 360);
                string strHex = angle.ToString("x8");

                angle = (Int32)(angleB * AxisBPulseCount / 360);
                string strHexB = angle.ToString("x8");
                strHex += strHexB;

                int speedTempA = Convert.ToInt32(speedA * AxisAPulseCount / 360);
                string strHexSpeedA = speedTempA.ToString("x8");
                strHex += strHexSpeedA;

                int speedTempB = Convert.ToInt32(speedB * AxisBPulseCount / 360);
                string strHexSpeedB = speedTempB.ToString("x8");
                strHex += strHexSpeedB;
                string stringSplit = Regex.Replace(strHex, @".{2}", "$0 ");//将string每两位分隔并添加空格                 
                string sendMessage = "AA AB 20 10 " + stringSplit + "FF";

                if (m_SocketMove.Connected)
                {
                    m_SocketMove.Send(Encoding.UTF8.GetBytes(sendMessage));
                    Debug.WriteLine(scancount + "," + sendMessage);
                    Thread.Sleep(100);
                    scancount++;
                }
               
                return true;
            }
            catch (Exception ex)
            {
                Debug.WriteLine("指令定位："+ex.ToString());
                return false;
            }
        }
        /// <summary>
        /// 指令定位，两个轴独立运动，分别设置速度
        /// </summary>
        /// <param name="angleA">目标位置</param>
        /// <param name="speed">运动速度</param>
        public bool MoveABTogether(double angleA, double angleB, double speed)
        {
            //double speedA = Convert.ToInt32(speed * 10 * 18000 * 100 * 4 / 360);
            isAmoving = true;
            if (composateState == 1)
            {
                angleA = (float)compensateA(angleA);
                angleB = (float)compensateB(angleB);
            }
          
            Int32 angle = (Int32)(angleA * AxisAPulseCount / 360);
            string strHex = angle.ToString("x8");
            angle = (Int32)(angleB * AxisBPulseCount / 360);
            string strHexB = angle.ToString("x8");
            strHex += strHexB;

            int speedTemp = Convert.ToInt32(speed * AxisAPulseCount / 360);
            string strHexSpeed = speedTemp.ToString("x8");
            strHex += strHexSpeed;
            string stringSplit = Regex.Replace(strHex, @".{2}", "$0 ");//将string每两位分隔并添加空格                 
            string sendMessage = "AA AB 22 0C " + stringSplit + "FF";

            if (m_SocketMove.Connected)
            {
                m_SocketMove.Send(Encoding.UTF8.GetBytes(sendMessage));
                Debug.WriteLine(scancount + "," + sendMessage);
                Thread.Sleep(100);
                scancount++;
            }
            return true;
        }
        /// <summary>
        /// 设置当前位置，
        /// 因控制卡无法自己获取编码器位置，
        /// 因此每次运动前需要先下发当前位置，
        /// 才可以执行脉冲定位指令
        /// </summary>
        /// <param name="angleA"></param>
        /// <param name="angleB"></param>
        /// <returns></returns>
        public bool SetCurrentPos(double angleA, double angleB)
        {
            if (composateState == 1)
            {
                angleA = (float)compensateA(angleA);
                angleB = (float)compensateB(angleB);
            }
          
            Int32 angle = (Int32)(angleA * AxisBPulseCount / 360);
            string strHexA = angle.ToString("x8");
            string strHexB = angle.ToString("x8");
            string strHex = strHexA + strHexB;
            string stringSplit = Regex.Replace(strHex, @".{2}", "$0 ");//将string每两位分隔并添加空格
            string sendMessage = "AA AB 21 08 " + stringSplit + "FF";
            if (m_SocketMove.Connected)
            {
                m_SocketMove.Send(Encoding.UTF8.GetBytes(sendMessage));
                Debug.WriteLine(sendMessage);
                return true;
            }
            else
                return false;
        }
        #endregion
        /// <summary>
        /// 球心指向,两个轴同时运动
        /// </summary>
        /// <param name="angleA">方位轴角度</param>
        /// <param name="angleB">俯仰轴角度</param>
        /// <param name="speed">速度</param>
        /// <returns>指令发送成功返回true，否则返回false</returns>
        public bool MoveAB(double angleA, double angleB, double speed)
        {
            double tempA = Math.Abs(angleA - a);
            double tempB = Math.Abs(angleB - b);
            int movespeed1 = Convert.ToInt32(speed * 10 * AxisAPulseCount / 360);
            int movespeed2 = (int)(movespeed1 * (tempB / tempA));
            //isMoveABComplete = false;
            bool isreversFw = false;
            bool isreversFy = false;
            angleA = (float)compensateA(angleA);
            if (angleA < 0)
            {
                isreversFw = true;
                angleA = -angleA;
            }
            Int32 angle = (Int32)(angleA * AxisBPulseCount / 360);
            string strHexA = angle.ToString("x6");
            if (isreversFw)
                strHexA = "80" + strHexA;
            else
                strHexA = "00" + strHexA;
            angleB = (float)compensateB(angleB);
            angleB += (float)fyZoro;
            if (angleB < 0)
            {
                isreversFy = true;
                angleB = -angleB;
            }
            angle = (Int32)(angleB * (18000 * 100 * 4) / 360);
            string strHexB = angle.ToString("x6");
            if (isreversFy)
                strHexB = "80" + strHexB;
            else
                strHexB = "00" + strHexB;
            string strHex = strHexA + strHexB + movespeed1.ToString("x8") + movespeed2.ToString("x8");
            string stringSplit = Regex.Replace(strHex, @".{2}", "$0 ");//将string每两位分隔并添加空格
            string sendMessage = "AA AB 22 0C " + stringSplit + "FF";
            if (m_SocketMove.Connected)
            {
                m_SocketMove.Send(Encoding.UTF8.GetBytes(sendMessage));
                Debug.WriteLine(sendMessage);
                return true;
            }
            else
                return false;
        }

        /// <summary>
        /// 连续测量开始
        /// </summary>
        public void StartMeas()
        {
            string sendMessage = CK_NETPANEL_LIANXU_START;
            m_socketMain.Send(Encoding.UTF8.GetBytes(sendMessage));
        }
        /// <summary>
        /// 连续测量结束
        /// </summary>
        public void EndMeas()
        {
            string sendMessage = CK_NETPANEL_LIANXU_END;
            m_socketMain.Send(Encoding.UTF8.GetBytes(sendMessage));
        }


        /// <summary>
        /// 修改控制卡的IP
        /// </summary>
        /// <param name="ip">控制卡的ip地址</param>
        public void ChangeIP_mainSocket(string ip)
        {
            //                     old-ip          new-ip
            //Net Config 000_02345_010.003.010.200_010.003.100_255.255.255.000
            //默认网口卡地址位10.3.10.200
            string mainIp = "Net Config 000_02345_010.003.010.200_" + ip + "_255.255.255.000";
            m_socketMain.Send(Encoding.UTF8.GetBytes(mainIp));
            LaserRadarLog.WriteLogInfo(typeof(DeviceRadar), "设置网口卡新ip：" + ip);
        }
        /// <summary>
        /// 修改信号卡的ip
        /// </summary>
        /// <param name="ip">信号卡的ip地址</param>
        public void ChaneIP_singleSocket(string ip)
        {
            //                     old-ip          new-ip
            //Net Config 000_02345_010.003.010.211_010.003.100_255.255.255.000
            //默认信号板地址位10.3.10.211
            string socketIP = "Net Config 000_02345_010.003.010.211_" + ip + "_255.255.255.000";
            m_socketSingal.Send(Encoding.UTF8.GetBytes(socketIP));
            LaserRadarLog.WriteLogInfo(typeof(DeviceRadar), "设置信号卡新ip：" + ip);
        }

        /// <summary>
        /// //带补偿的笛卡尔坐标转换
        /// </summary>
        /// <param name="fw">方位角度值</param>
        /// <param name="fy">俯仰角度值</param>
        /// <param name="dis">距离</param>
        /// <param name="X">补偿后的X</param>
        /// <param name="Y">补偿后的Y</param>
        /// <param name="Z">补偿后的Z</param>

        static public void ComputeXYZ(double fw, double fy, double dis, ref double X, ref double Y, ref double Z)
        {
            CompesateXYZ(ref X, ref Y, ref Z, dis, fw, fy);
        }
        /// <summary>
        /// 不带补偿的笛卡尔坐标转换
        /// </summary>
        /// <param name="fw">方位角度值</param>
        /// <param name="fy">俯仰角度值</param>
        /// <param name="dis">距离</param>
        /// <param name="X">转换后的X</param>
        /// <param name="Y">转换后的Y</param>
        /// <param name="Z">转换后的Z</param>

        static public void ComputeXYZ_noComse(double fw, double fy, double dis, ref double X, ref double Y, ref double Z)
        {
            fy = fy * 2 * Math.PI / 360;// 角度转弧度
            fw = fw * 2 * Math.PI / 360;
            fy = fy * 2;
            double x = dis * Math.Cos(fy) * Math.Cos(fw);
            double y = dis * Math.Cos(fy) * Math.Sin(fw);
            double z = dis * Math.Sin(fy);
            X = x;
            Y = y;
            Z = z;
        }

        /// <summary>
        /// 读取温度
        /// </summary>
        public void ReadTemperature()
        {
            string sendMessage = CK_READ_TEMPERATURE;
            try
            {
                if (m_socketMain.Connected)
                    m_socketMain.Send(Encoding.UTF8.GetBytes(sendMessage));
            }
            catch
            {                
            }
        }

        static public bool getScanData = false;
        static public bool isScan = false;
        static public bool scanComplete = true;
        public List<OriginData> findCenterData = new List<OriginData>();
        static CancellationTokenSource tokenSource;
        static ManualResetEvent _event = new ManualResetEvent(true);
        /// <summary>
        /// 区域扫描功能
        /// </summary>
        /// <param name="step">俯仰轴步距角度</param>
        /// <param name="speed">扫描速度</param>
        /// <param name="name"></param>
        public void ScanMove(double step, double speed)
        {
            moveSpeed = (float)speed;
            pathStep = (float)step;
            tokenSource = new CancellationTokenSource();
            CancellationToken token = tokenSource.Token;
            Task rectScanTask = new Task(RectMove, token);
            rectScanTask.Start();
            LaserRadarLog.WriteLogInfo(typeof(DeviceRadar), "开始区域扫描");
        }
        /// <summary>
        /// 孔扫描功能能
        /// </summary>
        /// <param name="speed">孔扫运动速度</param>
        public void HoleScan(double speed)
        {
            moveSpeed = (float)speed;
            tokenSource = new CancellationTokenSource();
            CancellationToken token = tokenSource.Token;
            Task holeScanTask = new Task(HoleMove, token);
            holeScanTask.Start();
            LaserRadarLog.WriteLogInfo(typeof(DeviceRadar), "开始测孔扫描");
        }
        /// <summary>
        /// 孔扫描运动
        /// </summary>
        private void HoleMove()
        {
            List<pathPoint> pathPoints = m_Path;
            int n = pathPoints.Count;
            pathPoint p;
           
            //按照指定路径点进行运动
            for (int i = 0; i < n; i++)
            {
                if (tokenSource.Token.IsCancellationRequested)//此线程取消
                {
                    break;
                }
                _event.WaitOne();
                p = pathPoints[i];
                //偶数点是直线的起始点
                if (i % 2 == 0)
                {
                    isMoveABComplete = false;
                    MoveAB((float)p.x,(float)p.y, moveSpeed);
                    while (!isMoveABComplete)
                    { }
                    //运动到起始点之后开始向主程序发送数据         
                    isScan = true;
                }
                //奇数点是直线的终止点
                if (i % 2 == 1)
                {
                    isMoveABComplete = false;
                    MoveAB((float)p.x, (float)p.y, moveSpeed);
                    while (!isMoveABComplete)
                    { }
                    //运动到终止点之后停止向主程序发送数据         
                    isScan = false;
                }
               
            }
            //需要告诉主程序停止接收数据
            SendStateValue("测孔扫描完成");
            LaserRadarLog.WriteLogInfo(typeof(DeviceRadar), "测孔扫描完成");
        }
        /// <summary>
        /// 多边形扫描功能
        /// </summary>
        /// <param name="speed">扫描速度</param>
        public void PolygenScan(double speed)
        {
            moveSpeed = (float)speed;
            tokenSource = new CancellationTokenSource();
            CancellationToken token = tokenSource.Token;
            Task polygenScanTask = new Task(PolygenMove, token);
            polygenScanTask.Start();
            LaserRadarLog.WriteLogInfo(typeof(DeviceRadar), "开始多边形扫描");
        }
        /// <summary>
        /// 多边形扫描运动
        /// </summary>
        private void PolygenMove()
        {
            List<pathPoint> pathPoints = m_Path;
            int n = pathPoints.Count;
            pathPoint p;
            //开始向主程序发送数据         
            isScan = true;
            //按照指定路径点进行运动
            for (int i = 0; i < n; i++)
            {
                if (tokenSource.Token.IsCancellationRequested)//此线程取消
                {
                    break;
                }
                _event.WaitOne();
                p = pathPoints[i];
                //偶数点是直线的起始点
                if (i % 2 == 0)
                {
                    isMoveAComplete = false;
                    AmoveTo((float)p.x, moveSpeed);
                    while (!isMoveAComplete)
                    { }
                    isMoveBComplete = false;
                    BmoveTo((float)p.y, moveSpeed);
                    while (!isMoveBComplete)
                    { }
                    //isMoveABComplete = false;
                    //MoveAB((float)p.x, (float)p.y, moveSpeed);
                    //while (!isMoveABComplete)
                    //{ }
                    //运动到起始点之后开始向主程序发送数据         
                    //isScan = true;
                }
                //奇数点是直线的终止点
                if (i % 2 == 1)
                {
                    isMoveAComplete = false;
                    AmoveTo((float)p.x, moveSpeed);
                    while (!isMoveAComplete)
                    { }
                    isMoveBComplete = false;
                    BmoveTo((float)p.y, moveSpeed);
                    while (!isMoveBComplete)
                    { }
                    //isMoveABComplete = false;
                    //MoveAB((float)p.x, (float)p.y, moveSpeed);
                    //while (!isMoveABComplete)
                    //{ }
                    //运动到终止点之后停止向主程序发送数据         
                    //isScan = false;
                }

            }
            //停止向主程序发送数据         
            isScan = false;
            //需要告诉主程序停止接收数据
            SendStateValue("多边形扫描完成");
            Debug.WriteLine("LaserRadar："+"多边形扫描完成");
            LaserRadarLog.WriteLogInfo(typeof(DeviceRadar), "多边形扫描完成");
        }
        /// <summary>
        /// 矩形扫描运动
        /// </summary>
        private void RectMove()
        {
            Class_PathGenerate pathGen = new Class_PathGenerate();
            //scanP1 = new pathPoint(3, -7);
            //scanP2 = new pathPoint(1, -8);
            List<pathPoint> getpoints = pathGen.generate_RectScanPath(scanP1, scanP2, pathStep);
            int n = getpoints.Count;
            pathPoint p = getpoints[0];
            float angleA = (float)p.x;
            float angleB = (float)p.y;
            isMoveABComplete = false;
            MoveAB(angleA, angleB, moveSpeed);
            while (!isMoveABComplete)
            { }
            //开始向主程序发送数据         
            isScan = true;
            //按照指定路径点进行运动
            for (int i = 1; i < n; i++)
            {
                if (tokenSource.Token.IsCancellationRequested)//此线程取消
                {
                    break;
                }
                _event.WaitOne();
                p = getpoints[i];
                if (i % 2 == 1)
                {
                    isMoveAComplete = false;
                    AmoveTo((float)p.x, moveSpeed);
                    while (!isMoveAComplete)
                    { }
                }
                if (i % 2 == 0)
                {
                    isMoveBComplete = false;
                    BmoveTo((float)p.y, moveSpeed);
                    while (!isMoveBComplete)
                    { }
                }
            }
            //需要告诉主程序停止接收数据
            SendStateValue("区域扫描完成");
            LaserRadarLog.WriteLogInfo(typeof(DeviceRadar), "区域扫描完成");
            //停止向主程序发送数据
            isScan = false;
        }
        /// <summary>
        /// 单点扫描功能
        /// </summary>
        /// <param name="xs">方位轴间隔</param>
        /// <param name="ys">俯仰轴间隔</param>
        /// <param name="speed">扫描速度</param>
        /// <param name="name"></param>
        public void StartSingleScan(double xs, double ys, double speed)
        {
            xstep = xs;
            ystep = ys;
            moveSpeed = (float)speed;
            tokenSource = new CancellationTokenSource();
            CancellationToken token = tokenSource.Token;
            Task singleScanTask = new Task(SingleScan, token);
            singleScanTask.Start();
            LaserRadarLog.WriteLogInfo(typeof(DeviceRadar), "开始单点扫描");
        }
        /// <summary>
        /// 单点测量功能
        /// </summary>
        public void StartSingleMeas()
        {
            Task singleMeasTask = new Task(() =>
            {
                SingleMeas();
            });
            singleMeasTask.Start();
            LaserRadarLog.WriteLogInfo(typeof(DeviceRadar), "单点测量");
        }
        /// <summary>
        /// 单点扫描运动
        /// </summary>
        private void SingleScan()
        {
            List<pathPoint> path = m_Path;
            int num = path.Count;
            Class_PathGenerate pathGen = new Class_PathGenerate();
            double[] x = new double[num];
            double[] y = new double[num];
            if (pathGen.CalculateClockDirection(path, false) == ClockDirection.Clockwise)
            {
                path.Reverse();
            }
            for (int i = 0; i < num; i++)
            {
                x[i] = path[i].x;
                y[i] = path[i].y;
            }
            List<pathPoint> getpoints = pathGen.generatePath(x, y, xstep, ystep);
            StreamWriter sw = new StreamWriter("pathPoints.txt");
            StreamWriter swActual = new StreamWriter("actualPoints.txt");
            foreach (pathPoint p in getpoints)
            {               
                sw.WriteLine(p.x+","+ p.y);              
            }
            sw.Close();
            
            if (path != null)
            {
                foreach (pathPoint p in getpoints)
                {

                    if (tokenSource.Token.IsCancellationRequested)
                    {
                        break;
                    }
                    _event.WaitOne();
                    //this.MdiParent.singleScanComplete = false;
                    isMoveABComplete = false;
                    MoveAB(p.x, p.y, moveSpeed);
                    while (!isMoveABComplete)
                    {      }                    
                    swActual.WriteLine(a.ToString("F4") + "," +b.ToString("F4"));
                    SingleMeas();
                }
            }
            swActual.Close();
            SendStateValue("单点扫描完成");
            LaserRadarLog.WriteLogInfo(typeof(DeviceRadar), "单点扫描完成");
        }
        /// <summary>
        /// 测量一个单点（取1000个点后滤波），并发送sendSingMeas信息，包含x、y、z坐标值
        /// </summary>
        public void SingleMeas()
        {
            pointList.Clear();
            isgettingOne = true;
            while (isgettingOne)
            { }
            Thread.Sleep(100);
            SAPoint apoint = GetOnePoint();
            if (SendSingleMeas != null)
                SendSingleMeas(apoint.x, apoint.y, apoint.z);
        }
        /// <summary>
        /// center为找到的球心坐标，包含方位轴角度和俯仰轴角度
        /// </summary>
        private pathPoint center;
        /// <summary>
        /// 按照回型方法找球心
        /// </summary>
        /// <param name="orgin"></param>
        /// <param name="n"></param>
        /// <param name="step"></param>
        /// <param name="findSpeed"></param>
        /// <returns></returns>
        private void FindCenter(pathPoint orgin, int n, double step, int findSpeed)
        {
            findCenterData.Clear();
            Class_PathGenerate pathGen = new Class_PathGenerate();
            List<pathPoint> getpoints = pathGen.generate_centerPath(orgin, n, step);
            scanComplete = false;
            Task findTask = new Task(() =>
            {
                getScanData = true;

                foreach (pathPoint p in getpoints)
                {
                    isMoveABComplete = false;
                    float angleA = (float)p.x;
                    float angleB = (float)p.y;

                    MoveAB(angleA, angleB, findSpeed);
                    while (!isMoveABComplete)
                    {
                        //Debug.WriteLine(angleA +"," + angleB);
                    }
                    Debug.WriteLine(angleA + "," + angleB);
                }
                Debug.WriteLine("findSphere");
                getScanData = false;
                scanComplete = true;
                double aTemp = 0, bTemp = 0, lightTemp = 0;
                StreamWriter centerSm = new StreamWriter("球心" + DateTime.Now.Hour.ToString() + DateTime.Now.Minute.ToString() + DateTime.Now.Second.ToString() + ".csv");
                foreach (OriginData od in findCenterData)
                {
                    if (od.light > lightTemp)
                    {
                        lightTemp = od.light;
                        aTemp = od.a;
                        bTemp = od.b;
                    }
                    centerSm.WriteLine(od.a.ToString("0.0000") + "," + od.b.ToString("0.0000") + "," + od.light.ToString("0"));
                    center = new pathPoint(aTemp, bTemp);
                }
                centerSm.Close();
            });
            findTask.Start();
        }
        /// <summary>
        /// 方位轴找中心
        /// </summary>
        /// <param name="orgin"> 其实点位置</param>
        /// <param name="step"> 步距</param>
        /// <param name="findSpeed"> 速度</param>
        /// <returns>
        /// 中心点坐标
        /// </returns>
        public pathPoint FindFWCenter(pathPoint orgin, double step, double findSpeed)
        {
            try
            {
                fwcenterData = new StreamWriter("fwCenterData.txt");

                findCenterData.Clear();
                float angleA = (float)(orgin.x - step);
                float angleB = (float)orgin.y;
                AmoveTo(angleA, findSpeed);
                while (isAmoving) { }
                // while (!isMoveAComplete) { }
                getScanData = true;
                angleA = (float)(orgin.x + step);
                angleB = (float)orgin.y;
                AmoveTo(angleA, findSpeed);
                while (isAmoving) { }
                getScanData = false;
                double lightTemp = 0, aTemp = 0, bTemp = 0;
                foreach (OriginData od in findCenterData)
                {
                    if (fwcenterData != null)
                        fwcenterData.WriteLine(od.a.ToString("0.0000") + "," + od.light.ToString("0") + "," + od.dis.ToString("0.0000"));
                    if (od.light > lightTemp)
                    {
                        lightTemp = od.light;
                        aTemp = od.a;
                        bTemp = od.b;
                        //if (fwcenterData != null)
                        //    fwcenterData.WriteLine(aTemp.ToString("0.0000") + "," + lightTemp.ToString("0"));
                    }
                }
                fwcenterData.Close();
                fwcenterData.Dispose();
                center = new pathPoint(aTemp, bTemp);
                AmoveTo((float)aTemp, findSpeed);
                while (isAmoving) { }
                if (SendFindSphereData != null)
                {
                    SendFindSphereData(center.x, center.y, findCenterData);
                }
                //if (this.MdiParent.m_FindSphereFM != null)
                //{
                //    this.MdiParent.m_FindSphereFM.Invoke(new EventHandler(delegate
                //    {
                //        this.MdiParent.m_FindSphereFM.showFwData(findCenterData);
                //    }));
                //}
                LaserRadarLog.WriteLogInfo(typeof(DeviceRadar), "找到方位中心:" + aTemp + "," + bTemp);
                return center;
            }
            catch (Exception ex)
            {
                LaserRadarLog.WriteLogError(typeof(DeviceRadar), ex.ToString());
                return center = new pathPoint(0, 0);                
            }
        }
        /// <summary>
        /// 俯仰轴找中心
        /// </summary>
        /// <param name="orgin">起始位置</param>
        /// <param name="step">步距</param>
        /// <param name="findSpeed">速度</param>
        /// <returns>
        /// 返回中心点坐标
        /// </returns>
        public pathPoint FindFYCenter(pathPoint orgin, double step, double findSpeed)
        {
            try
            {
                fycenterData = new StreamWriter("fyCenterData.txt");

                findCenterData.Clear();
                float angleA = (float)orgin.x;
                float angleB = (float)(orgin.y - step);

                BmoveTo(angleB, findSpeed);
                while (isBmoving) { }
                getScanData = true;
                angleA = (float)orgin.x;
                angleB = (float)(orgin.y + step);
                BmoveTo(angleB, findSpeed);
                while (isBmoving) { }
                getScanData = false;
                double lightTemp = 0, aTemp = 0, bTemp = 0;
                foreach (OriginData od in findCenterData)
                {
                    if (fycenterData != null)
                        fycenterData.WriteLine(od.b.ToString("0.0000") + "," + od.light.ToString("0") + "," + od.dis.ToString("0"));
                    if (od.light > lightTemp)
                    {
                        lightTemp = od.light;
                        aTemp = od.a;
                        bTemp = od.b;
                    }
                }
                fycenterData.Close();
                fycenterData.Dispose();
                center = new pathPoint(aTemp, bTemp);
                BmoveTo((float)bTemp, findSpeed);
                while (isBmoving) { }
                //if (this.MdiParent.m_FindSphereFM != null)
                //{
                //    this.MdiParent.m_FindSphereFM.Invoke(new EventHandler(delegate
                //    {
                //        this.MdiParent.m_FindSphereFM.showFyData(findCenterData);
                //    }));

                //}
                if (SendFindSphereData != null)
                {
                    SendFindSphereData(center.x, center.y, findCenterData);
                }
                LaserRadarLog.WriteLogInfo(typeof(DeviceRadar), "找到俯仰中心:" + aTemp + "," + bTemp);
                return center;
            }
            catch (Exception ex)
            {
                LaserRadarLog.WriteLogError(typeof(DeviceRadar), ex.ToString());
                return center = new pathPoint(0, 0);
            }
        }
        /// <summary>
        /// 自动找球心
        /// </summary>
        /// <param name="orgin">
        /// 初始点坐标
        /// </param>
        /// <param name="n">
        /// 圈数
        /// </param>
        /// <param name="step">
        /// 每圈步距
        /// </param>
        /// <param name="findSpeed">
        /// 运动速度
        /// </param>
        public void AutoFindCenter(pathPoint orgin, int n, double step, double findSpeed)
        {
            findCenterData.Clear();
            Class_PathGenerate pathGen = new Class_PathGenerate();
            List<pathPoint> getpoints = pathGen.generate_centerPath(orgin, n, step);
            scanComplete = false;
            float angleA, angleB;
            getScanData = true;
            foreach (pathPoint p in getpoints)
            {
                isMoveABComplete = false;
                angleA = (float)p.x;
                angleB = (float)p.y;
                MoveAB(angleA, angleB, findSpeed);
                while (!isMoveABComplete)
                {     }
                Debug.WriteLine(angleA + "," + angleB);
            }
            Debug.WriteLine("findSphere");
            getScanData = false;
            scanComplete = true;
            double aTemp = 0, bTemp = 0, lightTemp = 0;

            foreach (OriginData od in findCenterData)
            {
                if (od.light > lightTemp)
                {
                    lightTemp = od.light;
                    aTemp = od.a;
                    bTemp = od.b;
                    
                }
                center = new pathPoint(aTemp, bTemp);
            }
            AmoveTo((float)center.x, findSpeed);
            while (isAmoving) { }
            BmoveTo((float)center.y, findSpeed);
            while (isBmoving) { }
            Debug.WriteLine("回型完成:" + aTemp + "," + bTemp);

            double _radius = 19.05;
            OriginData firstCenter = GetOneData();
            double deltaA = Math.Atan(_radius / (_radius + firstCenter.dis)) * 180 / Math.PI *0.8;
            double deltaB = Math.Atan(_radius / (_radius + firstCenter.dis)) * 180 / Math.PI / 2 * 0.8;

            //水平方向找中心
            center = FindFWCenter(center, deltaA, findSpeed);
            Debug.WriteLine("方位中心已找到");
            //竖直方向找中心
            center = FindFYCenter(center, deltaB, findSpeed);
            Debug.WriteLine("俯仰中心已找到");

            AmoveTo((float)center.x, findSpeed);
            while (isAmoving) { }
            BmoveTo((float)center.y, findSpeed);
            while (isBmoving) { }
            SingleMeas();
            OriginData data =GetOneData();
            
            LaserRadarLog.WriteLogInfo(typeof(DeviceRadar), "任务完成:" + data.a + "," + data.b +","+data.dis);
        }
        //指定点测量
        public void MeasureTargets(pathPoint orgin, int n, double step, double findSpeed)
        {
            findCenterData.Clear();
            Class_PathGenerate pathGen = new Class_PathGenerate();
            List<pathPoint> getpoints = pathGen.generate_centerPath(orgin, n, step);
            scanComplete = false;
            float angleA, angleB;
            getScanData = true;

            foreach (pathPoint p in m_Path)
            {
                isMoveABComplete = false;
                angleA = (float)p.x;
                angleB = (float)p.y;
                MoveAB(angleA, angleB, findSpeed);
                while (!isMoveABComplete)
                { }
                Debug.WriteLine(angleA + "," + angleB);
                SingleMeas();
                //OriginData data = GetOneData();

                //LaserRadarLog.WriteLogInfo(typeof(DeviceRadar), "任务完成:" + data.a + "," + data.b + "," + data.dis);
            }
            //Debug.WriteLine("findSphere");
            //getScanData = false;
            //scanComplete = true;
            //double aTemp = 0, bTemp = 0, lightTemp = 0;

            //foreach (OriginData od in findCenterData)
            //{
            //    if (od.light > lightTemp)
            //    {
            //        lightTemp = od.light;
            //        aTemp = od.a;
            //        bTemp = od.b;

            //    }
            //    center = new pathPoint(aTemp, bTemp);
            //}
            //AmoveTo((float)center.x, findSpeed);
            //while (isAmoving) { }
            //BmoveTo((float)center.y, findSpeed);
            //while (isBmoving) { }
            //Debug.WriteLine("回型完成:" + aTemp + "," + bTemp);

            //double _radius = 19.05;
            //OriginData firstCenter = GetOneData();
            //double deltaA = Math.Atan(_radius / (_radius + firstCenter.dis)) * 180 / Math.PI * 0.8;
            //double deltaB = Math.Atan(_radius / (_radius + firstCenter.dis)) * 180 / Math.PI / 2 * 0.8;

            ////水平方向找中心
            //center = FindFWCenter(center, deltaA, findSpeed);
            //Debug.WriteLine("方位中心已找到");
            ////竖直方向找中心
            //center = FindFYCenter(center, deltaB, findSpeed);
            //Debug.WriteLine("俯仰中心已找到");

            //AmoveTo((float)center.x, findSpeed);
            //while (isAmoving) { }
            //BmoveTo((float)center.y, findSpeed);
            //while (isBmoving) { }
           
        }

        /// <summary>
        /// 补偿方位轴测量角度
        /// </summary>
        /// <param name="angle"> </param>
        /// <returns></returns>
        private double compensateA(double angle)
        {
            double angleA = 0;
            if (angle < 0)
            {
                angleA = angle + 360;
            }
            else
            {
                angleA = angle;
            }
            double afterCompen = pA[0] * Math.Sin(pA[1] * angleA + pA[2]) +
                                 pA[3] * Math.Sin(pA[4] * angleA + pA[5]) +
                                 pA[6] * Math.Sin(pA[7] * angleA + pA[8]) +
                                 pA[9] * Math.Sin(pA[10] * angleA + pA[11]) +
                                 pA[12];
            afterCompen = afterCompen / 3600;
            afterCompen += angle;
            return afterCompen;
        }

        private double disCompensateA(double angle)
        {
            double angleA = 0;
            if (angle < 0)
            {
                angleA = angle + 360;
            }
            else
            {
                angleA = angle;
            }
            double afterCompen = pA[0] * Math.Sin(pA[1] * angleA + pA[2]) +
                                 pA[3] * Math.Sin(pA[4] * angleA + pA[5]) +
                                 pA[6] * Math.Sin(pA[7] * angleA + pA[8]) +
                                 pA[9] * Math.Sin(pA[10] * angleA + pA[11]) +
                                 pA[12];
            afterCompen = afterCompen / 3600;
            afterCompen = angle - afterCompen;
            return afterCompen;
        }
        private double[] pA = new double[13];
        private double[] pB = new double[13];
        private double[] pA_dis = new double[13];
        private double[] pB_dis = new double[13];

        /// <summary>
        /// 补偿俯仰轴测量角度
        /// </summary>
        /// <param name="angle"></param>
        /// <returns></returns>
        private double compensateB(double angle)
        {
            double angleB = 0;
            if (angle < 0)
            {
                angleB = angle + 360;
            }
            else
            {
                angleB = angle;
            }
            double afterCompen = pB[0] * Math.Sin(pB[1] * angleB + pB[2]) +
                                 pB[3] * Math.Sin(pB[4] * angleB + pB[5]) +
                                 pB[6] * Math.Sin(pB[7] * angleB + pB[8]) +
                                 pB[9] * Math.Sin(pB[10] * angleB + pB[11]) +
                                 pB[12];
            afterCompen = afterCompen / 3600;
            afterCompen += angle;
            return afterCompen;
        }

        private double disCompensateB(double angle)
        {
            double angleB = 0;
            if (angle < 0)
            {
                angleB = angle + 360;
            }
            else
            {
                angleB = angle;
            }
            double afterCompen = pB[0] * Math.Sin(pB[1] * angleB + pB[2]) +
                                pB[3] * Math.Sin(pB[4] * angleB + pB[5]) +
                                pB[6] * Math.Sin(pB[7] * angleB + pB[8]) +
                                pB[9] * Math.Sin(pB[10] * angleB + pB[11]) +
                                pB[12];
            afterCompen = angle - afterCompen / 3600;
            return afterCompen;
        }
        /// <summary>
        /// 读取补偿信息，在\\LaserRadar\\目录下
        /// </summary>
        public void ReadCompensateList()
        {
            StreamReader srA = new StreamReader("C:\\LaserRadar\\fw.txt");
            StreamReader srB = new StreamReader("C:\\LaserRadar\\fy.txt");
            StreamReader srA_dis = new StreamReader("C:\\LaserRadar\\fw_dis.txt");
            StreamReader srB_dis = new StreamReader("C:\\LaserRadar\\fy_dis.txt");
            for (int i = 0; i < 13; i++)
            {
                pA[i] = Convert.ToDouble(srA.ReadLine());
                pB[i] = Convert.ToDouble(srB.ReadLine());
                pA_dis[i] = Convert.ToDouble(srA_dis.ReadLine());
                pB_dis[i] = Convert.ToDouble(srB_dis.ReadLine());
            }
            srA.Close();
            srB.Close();
            srA_dis.Close();
            srB_dis.Close();
        }


        /// <summary>
        /// 信号板设置延时参数
        /// </summary>
        /// <param name="delaytime1">延时1</param>
        /// <param name="delaytime2">延时2</param>
        public void setDelay(string delaytime1, string delaytime2)
        {
            string sendMessage = "FConfig " + delaytime1 + " " + delaytime2 + " " + "0400 6600 0400 6600 1024 FF";
            m_socketSingal.Send(Encoding.UTF8.GetBytes(sendMessage));
            m_socketSingal.Send(Encoding.UTF8.GetBytes("FMode0"));
        }
        /// <summary>
        /// 设置矩形扫描的起始点（已弃用）
        /// </summary>
        private void setStartPos()
        {
            string sendMessage = CK_TRUNTABLE_SM_POS_START;
            m_socketMain.Send(Encoding.UTF8.GetBytes(sendMessage));

        }
        /// <summary>
        /// 设置矩形扫描的终止点（已弃用）
        /// </summary>
        public void setEndPos()
        {
            string sendMessage = CK_TRUNTABLE_SM_POS_END;
            m_socketMain.Send(Encoding.UTF8.GetBytes(sendMessage));

        }
        /// <summary>
        /// 开始矩形扫描（已弃用）
        /// </summary>
        public void StartScan()
        {
            string sendMessage = CK_TRUNTABLE_SM_WORK_START;
            if (m_socketMain.Connected)
                m_socketMain.Send(Encoding.UTF8.GetBytes(sendMessage));

        }
        /// <summary>
        /// 暂停矩形扫描（已弃用）
        /// </summary>
        public void PauseScan()
        {
            string sendMessage = CK_TRUNTABLE_SM_WORK_PAUSE;
            if (m_socketMain.Connected)
                m_socketMain.Send(Encoding.UTF8.GetBytes(sendMessage));

        }
        /// <summary>
        /// 停止矩形扫描（已弃用）
        /// </summary>
        public void EndScan()
        {
            string sendMessage = CK_TRUNTABLE_SM_WORK_END;
            if (m_socketMain.Connected)
                m_socketMain.Send(Encoding.UTF8.GetBytes(sendMessage));
        }

        /// <summary>
        /// 暂停矩形扫描
        /// </summary>
        public void PauseMeas()
        {
            _event.Reset();
            SendStateValue("已暂停");
        }
        /// <summary>
        /// 继续矩形扫描
        /// </summary>
        public void ContinuMeas()
        {
            _event.Set();
        }
        /// <summary>
        /// 停止矩形扫描
        /// </summary>
        public void StopScan()
        {
            tokenSource.Cancel();
        }
        /// <summary>
        /// 发送方位运动完成信号
        /// </summary>
        private void SendAMoveComplete()
        {
            string sendMessage = CK_AMOVE_COMPELETE;
            m_socketMain.Send(Encoding.UTF8.GetBytes(sendMessage));
        }
        /// <summary>
        /// 发送方位运动完成信号
        /// </summary>
        private void SendBMoveComplete()
        {
            string sendMessage = CK_BMOVE_COMPELETE;
            m_socketMain.Send(Encoding.UTF8.GetBytes(sendMessage));
        }

        /// <summary>
        /// 停止信号板解算
        /// 标定水平仪时需要先暂停信号板解算
        /// </summary>
        //20211018添加水平仪功能
        public void StopSingal()
        {
            if (m_socketSingal != null)
            {
                m_socketSingal.Send(Encoding.UTF8.GetBytes(CK_STOP));
                Debug.WriteLine("停止水平仪信号接收");
            }
        }
        /// <summary>
        /// 开始信号板解算
        /// </summary>
        public void StartSingal()
        {
            SignalIndex = 0;
           // connectToSingal();

            m_socketSingal.Send(Encoding.UTF8.GetBytes(CK_START));
            Debug.WriteLine("开始信号接收");
        }
        /// <summary>
        /// 连接水平仪
        /// </summary>
        public void ConnectLevel()
        {
            //connectToSingal();

            m_socketSingal.Send(Encoding.UTF8.GetBytes(CK_LEVEL_CONNECT));
        }
        /// <summary>
        /// 初始化水平仪
        /// </summary>
        public void StartLevel()
        {
            SignalIndex = 1;
            m_socketSingal.Send(Encoding.UTF8.GetBytes(CK_LEVEL_START));
        }
        /// <summary>
        /// 停止水平仪通信
        /// </summary>
        public void StopLevel()
        {
            m_socketSingal.Send(Encoding.UTF8.GetBytes(CK_LEVEL_STOP));
        }
        /// <summary>
        /// SignalIndex代表信号板数据格式，
        /// 0代表雷达测量信息
        /// 1代表水平仪信息
        /// </summary>
        public int SignalIndex = 0;

        /// <summary>
        /// 水平仪信息解析
        /// </summary>
        /// <param name="data"></param>
        private void LevelDataProcess(string data)
        {
            try
            {
                //*****对34位数据进行解析
                string computeTemp = data.Substring(0, 8);//aaab240c                   
                computeTemp = data.Substring(8, 6);
                double level_x=Math.Round( computeLevel(computeTemp) -g_level_x,3);
                computeTemp = data.Substring(14, 6);
                double level_y = Math.Round(computeLevel(computeTemp) -g_level_y,3);            
                this.SendLevel(level_x, level_y);              
            }
            catch (Exception ex)
            {
                LaserRadarLog.WriteLogError(typeof(DeviceRadar), ex);
                Debug.WriteLine("信号处理出现错误：" + ex.ToString());
            }
        }

        /// <summary>
        /// 计算水平仪信息
        /// </summary>
        /// <param name="data"> </param>
        /// <returns>
        /// 返回当前轴的角度值
        /// </returns>
        private double computeLevel(string data)
        {
            double result = 0;
            if (data != null)
            {
                int value = Convert.ToInt32(data);
                if (value > 32767)
                    result = -(65536 - value) * 0.001;
                else
                    result = value * 0.001;
            }           
            return result;
        }
        /// <summary>
        /// 增量移动方位轴
        /// </summary>
        /// <param name="step">步进值</param>
        /// <param name="speed">移动速度</param>
        public void StepMoveA(float step,double speed =1.0)
        {         
            AmoveTo((float)a+step,speed);
        }
        /// <summary>
        ///增量移动俯仰轴
        /// </summary>
        /// <param name="step">步进值</param>
        /// <param name="speed">移动速度</param>
        public void StepMoveB(float step, double speed = 1.0)
        {
           BmoveTo((float)b + step, speed);
        }
        /// <summary>
        /// 设置距离阈值
        /// </summary>
        /// <param name="upValue">上限值</param>
        /// <param name="downValue">下限值</param>
        public void SetDistanceLimit(double upValue,double downValue)
        {
            upLimit = upValue;
            downLimit = downValue;
        }
        /// <summary>
        /// 多边形扫描路径规划
        /// </summary>
        /// <param name="xstep">方位步距</param>
        /// <param name="ystep">俯仰步距</param>
        /// <returns>规划成功返回true，失败返回false</returns>
        public List<pathPoint> generatePolygonScanPath(double xstep, double ystep,double angle)
        {

            List<pathPoint> path = m_Path;

            //先对边界点控制点进行旋转，旋转默认以第一个点为旋转中心
            List<pathPoint> vertex_points = Class_PathGenerate.two_demision_trans(path, angle);
            //StreamWriter sw0 = new StreamWriter("C:\\vertex.txt");
            //foreach (var p0 in vertex_points)
            //{
            //    sw0.WriteLine(p0.x + "," + p0.y + ",0");
            //}
            //sw0.Close();
            //记录旋转中心，为后续反变换使用
            double trans_center_x = path[0].x;
            double trans_center_y = path[0].y;

            double[] x = new double[vertex_points.Count];
            double[] y = new double[vertex_points.Count];
            int num = vertex_points.Count;
            Class_PathGenerate pathGen = new Class_PathGenerate();
            if (pathGen.CalculateClockDirection(vertex_points, false) == ClockDirection.Clockwise)
            {
                path.Reverse();
            }
            for (int i = 0; i < vertex_points.Count; i++)
            {
                x[i] = vertex_points[i].x;
                y[i] = vertex_points[i].y;
            }
            //根据步距值进行角度规划
            List<pathPoint> getpoints = pathGen.generatePolygonPath(x, y, xstep, ystep);
            //StreamWriter sw1 = new StreamWriter("C:\\path_ori.txt");
            //foreach (var p1 in getpoints)
            //{
            //    sw1.WriteLine(p1.x + "," + p1.y + ",0");
            //}
            //sw1.Close();

            //进行反变换
            List<pathPoint> trans_getpoints = Class_PathGenerate.path_trans_inverse(getpoints, trans_center_x, trans_center_y, angle);
            //StreamWriter sw = new StreamWriter("C:\\path.txt");
            //foreach (var p in trans_getpoints)
            //{
            //    sw.WriteLine(p.x + "," + p.y + ",0");
            //}
            //sw.Close();

            return trans_getpoints;
        }

        /// <summary>
        /// 圆孔扫描
        /// </summary>
        /// <param name="count">点数</param>
        /// <param name="radius">角度半径</param>
        /// <param name="currentA">当前方位角度</param>
        /// <param name="currentB">当前俯仰角度</param>
        /// <param name="targetA">圆孔中心方位角度</param>
        /// <param name="targetB">圆孔中心俯仰角度</param>
        /// <param name="speed">扫描速度</param>
        /// <returns></returns>
        public bool CircleScanMove(
            int count,
            double radius,
            double currentA,
            double currentB,
            double targetA, 
            double targetB, 
            double speed)
        {
                        
            if (composateState == 1)
            {
                currentA = compensateA(currentA);
                currentB = compensateB(currentB);
                targetA = compensateA(targetA);
                targetB = compensateB(targetB);
            }

            Int32 angle = 0;
            string strHex = null;

            strHex += count.ToString("x2");
            Int32 radiusInt = (Int32)(radius * AxisAPulseCount / 360);
            strHex += radiusInt.ToString("x8");
            angle   = (Int32)(currentA * AxisAPulseCount / 360);
            strHex += angle.ToString("x8");          
            angle = (Int32)(currentB * AxisBPulseCount / 360);
            strHex += angle.ToString("x8");
            angle = (Int32)(targetA * AxisAPulseCount / 360);
            strHex += angle.ToString("x8");
            angle = (Int32)(targetB * AxisBPulseCount / 360);
            strHex += angle.ToString("x8");
            int speedTemp = Convert.ToInt32(speed * AxisAPulseCount / 360);          
            strHex += speedTemp.ToString("x8");

            string stringSplit = Regex.Replace(strHex, @".{2}", "$0 ");//将string每两位分隔并添加空格                 
            string sendMessage = "AA AB 89 19 " + stringSplit + "FF";

            if (m_SocketMove.Connected)
            {
                m_SocketMove.Send(Encoding.UTF8.GetBytes(sendMessage));
                Debug.WriteLine(scancount + "," + sendMessage);
                Thread.Sleep(100);
                scancount++;
            }
            return true;
        }

        /// <summary>
        /// 螺旋线扫描
        /// </summary>
        /// <param name="count">点数</param>
        /// <param name="circles">扫描圈数</param>
        /// <param name="radius">角度半径</param>
        /// <param name="currentA">当前方位角度</param>
        /// <param name="currentB">当前俯仰角度</param>
        /// <param name="targetA">扫描中心方位角度</param>
        /// <param name="targetB">扫描中心俯仰角度</param>
        /// <param name="speed">扫描速度</param>
        /// <returns></returns>
        public bool SpinScanMove(
           int count,
           int circles,
           double radius,
           double currentA,
           double currentB,
           double targetA,
           double targetB,
           double speed)
        {
            if (composateState == 1)
            {
                currentA = compensateA(currentA);
                currentB = compensateB(currentB);
                targetA = compensateA(targetA);
                targetB = compensateB(targetB);
            }

            Int32 angle = 0;
            string strHex = null;

            strHex += count.ToString("x4");
            strHex += circles.ToString("x4");
            Int32 radiusInt = (Int32)(radius * AxisAPulseCount / 360);
            strHex += radiusInt.ToString("x8");
            angle = (Int32)(currentA * AxisAPulseCount / 360);
            strHex += angle.ToString("x8");
            angle = (Int32)(currentB * AxisBPulseCount / 360);
            strHex += angle.ToString("x8");
            angle = (Int32)(targetA * AxisAPulseCount / 360);
            strHex += angle.ToString("x8");
            angle = (Int32)(targetB * AxisBPulseCount / 360);
            strHex += angle.ToString("x8");
            int speedTemp = Convert.ToInt32(speed * AxisAPulseCount / 360);
            strHex += speedTemp.ToString("x8");

            string stringSplit = Regex.Replace(strHex, @".{2}", "$0 ");//将string每两位分隔并添加空格                 
            string sendMessage = "AA AB 85 1C " + stringSplit + "FF";

            if (m_SocketMove.Connected)
            {
                m_SocketMove.Send(Encoding.UTF8.GetBytes(sendMessage));
                Debug.WriteLine(scancount + "," + sendMessage);
                Thread.Sleep(100);
                scancount++;
            }
            return true;
        }

        /// <summary>
        /// 
        /// </summary>
        /// <param name="fast"></param>
        public void BmoveNeg(double speed)
        {
                string strHex = null;
                int speedTemp = Convert.ToInt32(speed * AxisBPulseCount / 360);
                strHex += speedTemp.ToString("x6");
                strHex = "80" + strHex;
                string stringSplit = Regex.Replace(strHex, @".{2}", "$0 ");//将string每两位分隔并添加空格                 
                string sendMessage = "AA AB 24 04 " + stringSplit + "FF";

                if (m_SocketMove.Connected)
                m_SocketMove.Send(Encoding.UTF8.GetBytes(sendMessage));
        }
        public void BmoveStop()
        {                   
            string sendMessage = "AA AB 24 04 00 00 00 00 FF";

            if (m_SocketMove.Connected)
                m_SocketMove.Send(Encoding.UTF8.GetBytes(sendMessage));
        }
        /// <summary>
        /// 
        /// </summary>
        /// <param name="fast"></param>

        public void BmovePos(double speed)
        {
            string strHex = null;
            int speedTemp = Convert.ToInt32(speed * AxisBPulseCount / 360);
            strHex += speedTemp.ToString("x6");         
            strHex = "00" + strHex;
            string stringSplit = Regex.Replace(strHex, @".{2}", "$0 ");//将string每两位分隔并添加空格                 
            string sendMessage = "AA AB 24 04 " + stringSplit + "FF";

            if (m_SocketMove.Connected)
                m_SocketMove.Send(Encoding.UTF8.GetBytes(sendMessage));
        }

        public void AmoveNeg(double speed)
        {
            string strHex = null;
            int speedTemp = Convert.ToInt32(speed * AxisBPulseCount / 360);
            strHex += speedTemp.ToString("x6");
            strHex = "80" + strHex;
            string stringSplit = Regex.Replace(strHex, @".{2}", "$0 ");//将string每两位分隔并添加空格                 
            string sendMessage = "AA AB 23 04 " + stringSplit + "FF";

            if (m_SocketMove.Connected)
                m_SocketMove.Send(Encoding.UTF8.GetBytes(sendMessage));
        }
        public void AmoveStop()
        {
            string sendMessage = "AA AB 23 04 00 00 00 00 FF";

            if (m_SocketMove.Connected)
                m_SocketMove.Send(Encoding.UTF8.GetBytes(sendMessage));
        }
        public void AmovePos(double speed)
        {
            string strHex = null;
            int speedTemp = Convert.ToInt32(speed * AxisBPulseCount / 360);
            strHex += speedTemp.ToString("x6");
            strHex = "00" + strHex;
            string stringSplit = Regex.Replace(strHex, @".{2}", "$0 ");//将string每两位分隔并添加空格                 
            string sendMessage = "AA AB 23 04 " + stringSplit + "FF";

            if (m_SocketMove.Connected)
                m_SocketMove.Send(Encoding.UTF8.GetBytes(sendMessage));
        }

        public void MoveStop()
        {
            string sendMessage = CK_MOVE_STOP;
            if (m_SocketMove.Connected)
                m_SocketMove.Send(Encoding.UTF8.GetBytes(sendMessage));
        }

        public void SetScale(double scale)
        {
            int scaleTemp = (int)scale;
            string strHex = null;
            strHex += scaleTemp.ToString("x4");

            string stringSplit = Regex.Replace(strHex, @".{2}", "$0 ");//将string每两位分隔并添加空格                 
            string sendMessage = "AA AB 26 " + stringSplit + "FF";

            if (m_SocketMove.Connected)
                m_SocketMove.Send(Encoding.UTF8.GetBytes(sendMessage));
        }

        public void SetRectangleScanSpeed(double speed)
        {
            string strHex = null;
            int speedTemp = Convert.ToInt32(speed * AxisBPulseCount / 360);
            strHex += speedTemp.ToString("x8");
            string stringSplit = Regex.Replace(strHex, @".{2}", "$0 ");//将string每两位分隔并添加空格                 
            string sendMessage = "AA AB 25 05 05 " + stringSplit + "FF";
            if (m_SocketMove.Connected)
                m_SocketMove.Send(Encoding.UTF8.GetBytes(sendMessage));
        }

        public void RectangleScanStart()
        {
            string sendMessage = CK_TRUNTABLE2_RECTANGLESCAN_ENTER;
            if (m_SocketMove.Connected)
                m_SocketMove.Send(Encoding.UTF8.GetBytes(sendMessage));
        }
        public void RectangleScanPause()
        {
            string sendMessage = CK_TRUNTABLE2_RECTANGLESCAN_PAUSE;
            if (m_SocketMove.Connected)
                m_SocketMove.Send(Encoding.UTF8.GetBytes(sendMessage));
        }
        public void RectangleScanContinue()
        {
            string sendMessage = CK_TRUNTABLE2_RECTANGLESCAN_CONTINUE;
            if (m_SocketMove.Connected)
                m_SocketMove.Send(Encoding.UTF8.GetBytes(sendMessage));
        }

        public void RectangleScanAddPoints(List<pathPoint>  points)
        {
            int count = points.Count();
            if (count > 10)
            {
                LaserRadarLog.WriteLogInfo(typeof(DeviceRadar), "添加边界点的数量大于10");
                return;
            }
            string temp = null;
            temp += count.ToString("x2");
            foreach (var p in points)
            {
                double x = p.x;
                double y = p.y;
                if (composateState == 1)
                {
                    x = (float)compensateA(x);
                    y = (float)compensateB(y);
                }
                temp += ConvertAngle2String(x,y);
            }
            string stringSplit = Regex.Replace(temp, @".{2}", "$0 ");//将string每两位分隔并添加空格                 
            string sendMessage = "AA AB 25 0F 01 " + stringSplit + "FF";
            if (m_SocketMove.Connected)
                m_SocketMove.Send(Encoding.UTF8.GetBytes(sendMessage));
        }

        public string ConvertAngle2String(double angleA,double angleB)
        {
            string strHexA = null;
            bool isreversFw = false;
            if (angleA < 0)
            {
                isreversFw = true;
                angleA = -angleA;
            }
            Int32 angle = (Int32)(angleA * AxisAPulseCount / 360);
            strHexA += angle.ToString("x6");
            if (isreversFw)
                strHexA = "80" + strHexA;
            else
                strHexA = "00" + strHexA;

            bool isreversFy = false;
            angle = (Int32)(angleB * AxisBPulseCount / 360);
            if (angle < 0)
            {
                isreversFy = true;
                angle = -angle;
            }
            string strHexB = angle.ToString("x6");
            if (isreversFy)
                strHexB = "80" + strHexB;
            else
                strHexB = "00" + strHexB;         
            return strHexA + strHexB;
        }

        /// <summary>
        /// 向控制板下发扫描边界点，每次最多发送10个边界点
        /// </summary>
        public void ScanAddPoints()
        {
            List<pathPoint> path = new List<pathPoint>();
            int n = m_Path.Count;
            if (n <= 0)
            {
                return;
            }
            if (n > 10)
                n = 10;
            
            for (int i = 0; i < n; i++)
            {
                path.Add(m_Path.First());
                m_Path.RemoveAt(0);
            }
            RectangleScanAddPoints(path);
        }

        public void RedLight_ON()
        {
            string sendMessage = CK_APPENDIX_REDLIGHT_ON;
            if (m_socketMain.Connected)
            {
                m_socketMain.Send(Encoding.UTF8.GetBytes(sendMessage));
                LaserRadarLog.WriteLogInfo(typeof(DeviceRadar), "红灯带开");
            }
        }
        public void RedLight_OFF()
        {
            string sendMessage = CK_APPENDIX_REDLIGHT_OFF;
            if (m_socketMain.Connected)
            {
                m_socketMain.Send(Encoding.UTF8.GetBytes(sendMessage));
                LaserRadarLog.WriteLogInfo(typeof(DeviceRadar), "红灯带关");
            }
        }
        public void GreenLight_ON()
        {
            string sendMessage = CK_APPENDIX_GREENLIGHT_ON;
            if (m_socketMain.Connected)
            {
                m_socketMain.Send(Encoding.UTF8.GetBytes(sendMessage));
                LaserRadarLog.WriteLogInfo(typeof(DeviceRadar), "绿灯带开");
            }
        }
        public void GreenLight_OFF()
        {
            string sendMessage = CK_APPENDIX_GREENLIGHT_OFF;
            if (m_socketMain.Connected)
            {
                m_socketMain.Send(Encoding.UTF8.GetBytes(sendMessage));
                LaserRadarLog.WriteLogInfo(typeof(DeviceRadar), "绿灯带关");
            }
        }
        public void BlueLight_ON()
        {
            string sendMessage = CK_APPENDIX_BULELIGHT_ON;
            if (m_socketMain.Connected)
            {
                m_socketMain.Send(Encoding.UTF8.GetBytes(sendMessage));
                LaserRadarLog.WriteLogInfo(typeof(DeviceRadar), "蓝灯带开");
            }
        }
        public void BlueLight_OFF()
        {
            string sendMessage = CK_APPENDIX_BULELIGHT_OFF;
            if (m_socketMain.Connected)
            {
                m_socketMain.Send(Encoding.UTF8.GetBytes(sendMessage));
                LaserRadarLog.WriteLogInfo(typeof(DeviceRadar), "蓝灯带关");
            }
        }
        public void ChannelOn(int index)
        {
            string strHex = index.ToString("x2");                
            string sendMessage = "AA AB OS " + strHex + " FF";
            if (m_socketMain.Connected)
            {
                m_socketMain.Send(Encoding.UTF8.GetBytes(sendMessage));
                LaserRadarLog.WriteLogInfo(typeof(DeviceRadar), "开启通道："+index);
            }
        }
        public void LimitSelect(string value)
        {          
            string sendMessage = "AA AB LMT " + value + " FF";
            if (m_socketMain.Connected)
            {
                m_socketMain.Send(Encoding.UTF8.GetBytes(sendMessage));
                LaserRadarLog.WriteLogInfo(typeof(DeviceRadar), "选择限位开关:"+value);
            }
        }
        //public void ChannelOff(int index)
        //{
        //    string strHex = index.ToString("x2");
        //    string sendMessage = "AA AB OS" + strHex + " ON FF";
        //    if (m_socketMain.Connected)
        //    {
        //        m_socketSingal.Send(Encoding.UTF8.GetBytes(sendMessage));
        //        LaserRadarLog.WriteLogInfo(typeof(DeviceRadar), "蓝灯带关");
        //    }
        //}
        /// <summary>
        /// 读取温度、湿度、压力
        /// </summary>
        public void ReadSensorData()
        {
            string sendMessage = CK_APPENDIX_SENSOR_REQUEST;         
            if (m_socketMain.Connected)
                   m_socketMain.Send(Encoding.UTF8.GetBytes(sendMessage));
        }
        public void ReadCyberTemper()
        {
            string sendMessage = CK_APPENDIX_CYBERETHERMOMETER_REQUEST;
            if (m_socketMain.Connected)
                m_socketMain.Send(Encoding.UTF8.GetBytes(sendMessage));
        }
        public void SetATTENUATOR(double value)
        {
            Int32 temp =( Int32)(value * 1000);
            string strHex = temp.ToString("0000");
            //string stringSplit = Regex.Replace(strHex, @".{2}", "$0 ");
            string sendMessage = "AA AB VOA AB VOL " + strHex + "FF";
            if (m_socketMain.Connected)
            {
                m_socketMain.Send(Encoding.UTF8.GetBytes(sendMessage));
            }
        }
    }
   
}
